#include <math.h>
#include "timer.h"
#include "world.h"
#include "task_manager.h"
#include "public_kernel.h"
#include "z_types.h"
#include <agv_msgs/msg/state.hpp>
#include <agv_msgs/msg/map_node.hpp>
// operation
#include "operation_charge.h"
#include "operation_peripheral.h"
#include "operation_turn_around_once.h"
#include "operation_turn_around_omni.h"
#include "operation_waitfor_release.h"
#include "operation_pallet_identify.h"
#include "operation_cross_floor.h"
#include "public_macro.h"
#include "public_helper.h"
#include "follow_path.h"
#include "check_manager.h"
#include "timestamp.h"
#include <sstream>
// 流载入
#define OSS_TO_STRING(result, ...) \
    do                             \
    {                              \
        std::ostringstream oss;    \
        oss << __VA_ARGS__;        \
        result = oss.str();        \
    } while (0)
//
//
extern World world;
extern Timer timer;
extern WorkStatusRecord VhcStatus;
constexpr int FORCE_RELOCATE_PATH_BITS = 15;  // 强行路径重定位检测
constexpr int INDENTIFY_NODE_BITS = 13;  // 栈板识别节点bit位
constexpr float RELOACATE_SOCRE_LIMIT = 10.0f;
// #define MAX_STEER_ANGLE_SMOOTH 15.0f        // 平滑判断舵角度阈值
constexpr float MAX_HEAD_ANGLE_SMOOTH = 15.0f; // 最大平滑车头角度阈值
//////////////////////////////////////////////////////////////////////
// 线程安全函数
//    The timer task for navigation generation.
void MainTimerTask(DWORD dw_param)
{
    TaskManager *manager = (TaskManager *)dw_param;
    if (manager) {
        manager->TimerTask();
    }
}

////////////////////////////////Class ZoneSpinData implement//////////////////////////////////////
ZoneSpinData::ZoneSpinData()
                        : zone_spin_(false), nav_node_(agv_msgs::msg::MapNode()) 
{
    DEBUG_OUT("ZoneSpinData: zone_spin_=" << zone_spin_ << ", nodeid=" << nav_node_.nodeid);
}

ZoneSpinData::ZoneSpinData(bool spin, const agv_msgs::msg::MapNode& nav_node)
                        : zone_spin_(spin), nav_node_(nav_node) 
{
    DEBUG_OUT("ZoneSpinData: zone_spin_=" << zone_spin_ << ", nodeid=" << nav_node_.nodeid);
}

ZoneSpinData::ZoneSpinData(const agv_msgs::msg::MapNode& nav_node)
                         : zone_spin_(false), nav_node_(nav_node) 
{
    DEBUG_OUT("helper: ZoneSpinData, nodeid=" << nav_node_.nodeid);
}

/**
 * @brief 获取角度
 *
 * 从ZoneSpinData对象中获取角度。
 *
 * @return 返回计算得到的角度值，类型为float。
 */
float ZoneSpinData::GetAngle() 
{
    float angle = 0.0f;
    std::string node_id = nav_node_.nodeid;
    int to_node = 0;
    if (!string2Number(node_id, to_node)) {
        DEBUG_OUT("error: ZoneSpinData: GetAngle failed; nodeid is not number; nodeid=" << node_id);
        return angle;
    }

    Node* spin_node = world.GetNode(to_node);
    if (!spin_node) {
        DEBUG_ERROR_OUT("error: ZoneSpinData get node failed; pSpinNode is null");
        return angle;
    }

    angle = GetAdjustAngle(*spin_node, SelectNodeAngleType());
    return angle;
}

/**
 * @brief 选择节点的角度类型
 *
 * 根据导航节点的动作列表和动作类型，选择节点的角度类型。
 *
 * @return 返回选择的角度类型（整数表示）。
 */
int ZoneSpinData::SelectNodeAngleType()
{
    int type = (int)AdjustAngleType::AdTypeNormal;
    const auto& actions = nav_node_.actions;

    if (actions.size() < 1) {
        DEBUG_ERROR_OUT("SelectNodeAngleType action list is empty, actions.size=" << actions.size());
        return type;
    }
    const auto& action = actions[0];
    std::string block_type = action.blockingtype;

    if (action.actionid.empty() || action.actiontype.empty()) {
        DEBUG_ERROR_OUT("SelectNodeAngleType: nodeid=" << nav_node_.nodeid
                                                        << ", none action: actionid:" << action.actionid
                                                        << ", actiontype:" << action.actiontype);
        return type;
    }

    if (block_type != OP_MUTEX_TYPE) {
        DEBUG_ERROR_OUT("SelectNodeAngleType: nodeid=" << nav_node_.nodeid
                                << ", blockType != OP_MUTEX_TYPE, blockType=" << block_type);
        return type;
    }

    if (action.actiontype == ACTION_PICK) {
        type = (int)AdjustAngleType::AdTypePick;
        DEBUG_OUT("selectNodeAngleType is typePick");
    } else if (action.actiontype == ACTION_DROP) {
        type = (int)AdjustAngleType::AdTypeDrop;
        DEBUG_OUT("selectNodeAngleType is typeDrop");
    } else {
        type = (int)AdjustAngleType::AdTypeNormal;
        DEBUG_OUT("selectNodeAngleType is typeNormal");
    }

    DEBUG_OUT("ok SelectNodeAngleType: type=" << type
                        << ", actiontype:" << action.actiontype
                        << ", actionid:" << action.actionid);
    return type;
}

/**
 * @brief 获取调整角度
 *
 * 根据传入的节点和角度类型，获取相应的调整角度。
 *
 * @param node 节点对象
 * @param angle_type 角度类型，AdjustAngleType枚举值
 *
 * @return 返回相应的调整角度
 */
float ZoneSpinData::GetAdjustAngle(Node node, const int &angle_type)
{
    float head_nomal = node.heading_, head_pick = 0.0f, head_drop = 0.0f;
    node.GetActionHead(head_pick, head_drop);
    float angle = (angle_type == (int)AdjustAngleType::AdTypeNormal) ? head_nomal :
                    ((angle_type == (int)AdjustAngleType::AdTypePick) ? head_pick : head_drop);
    DEBUG_OUT("GetAdjustAngle: nodeid=" << node.id_ << ", angleType=" << angle_type
                                        << ", fAngle=" << angle);
    return angle;
}

//////////////////////////////////////////////////////////////////////
//   Part 1. Basic operations.
/**
 * @brief 开始导航
 *
 * 开始或停止导航任务
 *
 * @param yes 是否开始导航，true表示开始，false表示停止
 * @param online 是否在线模式，true表示在线模式，false表示离线模式
 */
void TaskManager::StartNavigation(bool yes, bool online) {
    DEBUG_OUT("StartNavigation start =" << yes);

    if (yes == start_) {
        DEBUG_ERROR_OUT("Error: Set navigation failed: state is the same; "
                        "set new start: yes=" << yes);
        return;
    }

    DEBUG_OUT("Ready start nav yes=" << yes);

    // 获取函数运行锁
    std::lock_guard<std::mutex> locker(timer.function_mutex_);
    InitValue();

    if (yes) {
        timer.AssignTask(1, MainTimerTask, (DWORD)(this), TRUE);
    } else {
        timer.ClearTask(1);
    }

    TrajNv::init_posture_known_ = false;
    online_ = online;
    start_ = yes;

    DEBUG_OUT("Start nav yes=" << yes << " finish");
}

/**
 * @brief 设置暂停编译标志
 *
 * 设置是否暂停当前编译任务的标志。
 *
 * @param pause 是否暂停编译的标志，true 表示暂停，false 表示继续
 */
void TaskManager::SetPauseCompileFlag(bool pause) 
{
    pause_cur_compile_ = pause;
    DEBUG_OUT("SetPauseCompileFlag: pause=" << pause);
}

/**
 * @brief 初始化TaskManager对象的各个属性值
 *
 * 该函数用于初始化TaskManager对象的各个属性值，包括配置文件模式、曲率、上次轨迹类型、
 * 曲率是否已知、上次转向角度、是否使用速度等级、是否在线、任务错误详情、工作状态、
 * 车道变换结束任务标志、编译暂停标志、侧边平滑处理对象、任务清空等。
 *
 * @note 使用std::lock_guard<std::mutex>进行线程同步，确保在初始化过程中不会有其他线程同时访问该对象。
 */
void TaskManager::InitValue() 
{
  std::lock_guard<std::mutex> locker(run_obj_mutex_);
  DEBUG_OUT("Task manager InitValue");

  profile_mode_ = NV_NORMAL;
  curvature_ = 0.0f;
  last_traj_type_ = static_cast<int>(TrajType::UNDEFINED_TRAJ);
  curvature_known_ = false;
  last_steer_angle_ = Angle(0);
  use_speed_level_ = true;
  online_ = false;
  task_err_detail_.clear();
  work_state_ = AGV_IS_IDLE;
  lc_end_task_ = false;
  
  SetPauseCompileFlag(false);
  side_sooth_.Clear();
  ClearAllTask();  // reset
  
  DEBUG_OUT("Task manager InitValue ok");
}

/**
 * @brief TaskManager类的构造函数
 *
 * 初始化TaskManager类的各个成员变量，并启动一个线程用于处理任务。
 */
TaskManager::TaskManager() 
                : spin_turn_dir_(SMART_TURN_DIR),
                cur_task_id_(65535),
                cur_sub_task_id_(65535),
                can_compile_one_new_task_(false),
                cur_nv_(nullptr),
                cur_op_(nullptr), 
                cur_dyn_op_(nullptr),
                suspend_for_dyn_op_(false),
                start_(false) 
{
    task_id_buffer_.Create(TASK_BUFFER_NUM);
    // 创建vda5050主订单任务代码环形缓冲区
    order_task_buffer_.Create(TASK_BUFFER_NUM);
    // 创建导航代码环形缓冲区
    work_buffer_.Create(WORK_BUFFER_NUM);
    work_buffer_copy_.Create(WORK_BUFFER_NUM);

    cur_work_code_.Nullify();

    // 初始化用于预编译的动态虚拟姿态状态
    dyn_virt_status_.Init(&world, 3);  // nVelLevel = 3
    record_last_nv_vel_.SetZero();

    InitValue();
  
    // 打开初始化CAN线程
    quit_mutex_.lock();
    thread_ = std::thread(&TaskManager::ThreadFun, this);
}

TaskManager::~TaskManager()
{
    quit_mutex_.unlock();
    thread_.join();
}

/**
 * @brief 线程函数，用于处理任务管理器的线程任务
 *
 * 此函数是任务管理器的线程函数，用于执行循环任务。
 *
 * @param task_manager 指向任务管理器的指针
 */
void TaskManager::ThreadFun(TaskManager* task_manager) 
{
    if (!task_manager) return;

    while (!task_manager->quit_mutex_.try_lock_for(std::chrono::milliseconds(50))) {
        task_manager->ThreadCycle();
    }
}

/**
 * @brief 线程周期函数
 *
 * 该函数是 TaskManager 类中的成员函数，用于处理线程周期性的任务。
 *
 */
void TaskManager::ThreadCycle()
{
    if (!online_) {
        // 离线时,加入订单互斥锁
        std::lock_guard<std::mutex> locker(order_mutex_);
        if (order_task_buffer_.IsEmpty() && local_order_.Recycle() &&
                !local_order_.order.nodes.empty()) {
            order_task_buffer_.AddData(local_order_.order);
            DEBUG_OUT("LocalOrder.Recycle Add Data");
        }
    }
}

/**
 * @brief 设置初始化状态
 *
 * 设置车辆状态，并更新动态虚拟状态、级别和任务状态。
 *
 * @param vhc_status 车辆状态记录
 */
void TaskManager::SetInitState(WorkStatusRecord& vhc_status)
{
    // The stored location information updates the online data,
    // which may be in the middle of the road section.
    dyn_virt_status_ = vhc_status;
    level_ = vhc_status.level_;
    record_last_nv_vel_ = dyn_virt_status_.velocity_;
    task_status_ = START_WORKCODE;  // The next work code will be the start of a task.
    DEBUG_OUT("nav: initPos, curPos: x,y,theta="
            << vhc_status.cur_posture_.x_ << " " 
            << vhc_status.cur_posture_.y_ << " " 
            << vhc_status.cur_posture_.theta_);
}

/**
 * @brief 更新当前动态虚拟位置
 *
 * 更新当前动态虚拟位置，通过给定的姿态信息更新动态虚拟状态的位置。
 *
 * @param pst 姿态信息，包含位置和方向等信息
 */
void TaskManager::UpdataCurDynVirtulPos(const Posture& pst) 
{
    dyn_virt_status_.UpdateCurPos(pst);
}

//////////////////////////////////////////////////////////////////////////////
//   PART 2. Task compilation.
/**
 * @brief 根据给定的区域集ID和当前节点，判断当前节点是否可以旋转
 *
 * 根据给定的区域集ID和当前节点，判断当前节点是否可以旋转。
 *
 * @param zone_set_id 区域集ID，字符串类型
 * @param cur_node 当前节点，类型为agv_msgs::msg::MapNode
 *
 * @return 如果当前节点可以旋转，则返回true；否则返回false
 */
bool TaskManager::ZoneNodeSpin(const std::string& zone_set_id, agv_msgs::msg::MapNode cur_node) 
{
    // Check to insert spin when task is end.
    bool ret = false;
    int zone_set_id_num = 0;
    if (!string2Number(zone_set_id, zone_set_id_num)) {
        DEBUG_ERROR_OUT("Error: zonesetid to int failed; zonesetid:" << zone_set_id);
        return ret;
    }

    std::string node_id = cur_node.nodeid;
    int node_id_num = 0;
    if (!string2Number(node_id, node_id_num)) {
        DEBUG_OUT("Error: ZoneNodeSpin failed; nodeid:" << node_id);
        return ret;
    }

    ret = zone_set_id_num == node_id_num;
    if (ret) {
        Node* node = world.GetNode(zone_set_id_num);
        if (node) {
            Node zone_node = *node;
            // 辅助查找到点旋转角度类型
            using AngleType = ZoneSpinData::AdjustAngleType;
            ZoneSpinData tmp_helper_zone_data(cur_node);
            int type = tmp_helper_zone_data.SelectNodeAngleType();
            bool can_rotate = false;
            switch (type) {
                case (int)AngleType::AdTypeNormal:
                    // 是否开启了节点角度自旋转
                    can_rotate = zone_node.EnableNodeAngle();
                    break;
                case (int)AngleType::AdTypePick:
                    can_rotate = zone_node.EnablePickAngle();
                    break;
                case (int)AngleType::AdTypeDrop:
                    can_rotate = zone_node.EnableDropAngle();
                    break;
            }
            ret = can_rotate;
            DEBUG_OUT("ZoneNodeSpin: CanRotate =" << can_rotate
                    << ", zonesetid=" << zone_set_id_num
                    << ", nodeid=" << node_id_num
                    << ", type(Normal=0, pick=1, drop=2)=" << type);
        } else {
            ret = false;
            DEBUG_ERROR_OUT("Error: node is null; world cannot find zonesetid ="
                                << zone_set_id_num << ", nodeid=" << node_id_num);
        }
    }
    DEBUG_OUT("ZoneNodeSpin? =" << ret << ", zonesetid=" << zone_set_id_num
                << ", nodeid=" << node_id_num);
    return ret;
}

/**
 * @brief 获取指定区域集ID的节点单词
 *
 * 根据给定的区域集ID，获取对应节点的单词信息，并设置导航路径点的单词数据。
 *
 * @param zone_set_id 区域集ID
 *
 * @return 如果成功获取节点单词信息，则返回true；否则返回false
 */
bool TaskManager::ZoneNodeWords(const std::string& zone_set_id) 
{
    // 检查是否在任务结束时插入自旋
    bool ret = false;
    int zone_set_id_num = 0;

    if (!string2Number(zone_set_id, zone_set_id_num)) {
        DEBUG_ERROR_OUT("Error: zoneNodeWords zonesetid to int failed; zonesetid: " 
                        << zone_set_id);
        return ret;
    }

    Node* node = world.GetNode(zone_set_id_num);

    if (node) {
        Node zone_node = *node;
        // 是否开启了节点角度自旋转
        int word1 = -1, word2 = -1;
        zone_node.Words(word1, word2);
        SetNavPathPointWordData(word1, word2);
        DEBUG_OUT("zoneNodeWords: word1 = " << word1 << ", word2 = " << word2);
    } else {
        ret = false;
        SetNavPathPointWordData(-1, -1);
        DEBUG_ERROR_OUT("Error: zoneNodeWords: node is null; world cannot find zonesetid = "
                        << zone_set_id_num << ", clear word1 and word2 to -1");
    }

    return ret;
}

/**
 * @brief 日志记录当前订单信息
 *
 * 该函数用于将当前订单的信息以日志的形式输出。
 *
 * @param order 订单信息结构体
 */
void TaskManager::LogOutCurOrder(const agv_msgs::msg::Order order) {
    std::string order_id = order.orderid;
    uint32_t order_update_id = order.orderupdateid;
    std::string zone_set_id = order.zonesetid;
    std::string serial_number = order.serialnumber;
    std::string timestamp = order.timestamp;

    DEBUG_OUT("Log out order info: serialnumber: " << serial_number 
                << ", orderid: " << order_id 
                << ", orderupdateid: " << order_update_id
                << ", zonesetid: " << zone_set_id 
                << ", timestamp: " << timestamp);

    std::vector<agv_msgs::msg::MapNode> nodes = order.nodes;
    std::size_t node_count = nodes.size();

    DEBUG_OUT("nodes.size: " << node_count);

    for (std::size_t i = 0; i < node_count; ++i) {
        agv_msgs::msg::MapNode cur_node = nodes[i];
        DEBUG_OUT("i: " << i 
                    << ", nodeid: " << cur_node.nodeid 
                    << ", sequenceid: " << cur_node.sequenceid);

        std::vector<agv_msgs::msg::Action> actions = cur_node.actions;
        DEBUG_OUT("action.size: " << actions.size());

        for (std::size_t j = 0; j < actions.size(); ++j) {
            agv_msgs::msg::Action cur_action = actions[j];
            DEBUG_OUT("j: " << j 
                    << ", actiontype: " << cur_action.actiontype
                    << ", actionid: " << cur_action.actionid
                    << ", blockingtype: " << cur_action.blockingtype);
            
            std::vector<agv_msgs::msg::ActionParam> cur_action_param = cur_action.actionparameters;
            DEBUG_OUT("param.size: " << cur_action_param.size());
            
            for (std::size_t z = 0; z < cur_action_param.size(); ++z) {
                agv_msgs::msg::ActionParam param = cur_action_param[z];
                DEBUG_OUT("z: " << z << ", param: key: " << param.key << ", value: " << param.value);
            }
        }
    }
}

/**
 * @brief 判断是否为单点任务
 *
 * 判断给定的任务是否为单点任务。单点任务是指任务只包含一个节点，且该节点没有执行动作。
 *
 * @param order 任务信息
 * @return 如果任务为单点任务，则返回true；否则返回false。
 */
bool TaskManager::SingleNodeTask(const agv_msgs::msg::Order& order) {
    std::vector<agv_msgs::msg::MapNode> nodes = order.nodes;
    // #1 数量不是1:非单点任务
    int node_count = nodes.size();

    if (node_count != 1) {
        DEBUG_OUT("current is not single node task node_count = " << node_count);
        return false;
    }

    // #2 动作都非空:非单点任务
    auto cur_node = nodes[0];
    auto actions = cur_node.actions;
    bool action_valid = false;

    for (const auto& action : actions) {
        bool valid = (!action.actionid.empty()) && (!action.actiontype.empty());
        action_valid |= valid;
    }

    if (action_valid) {
        DEBUG_OUT("current is not single node task actions.size = " << actions.size());
        return false;
    }

    // #3 目标节点转换失败
    int to_node = 0;

    if (!string2Number(cur_node.nodeid, to_node)) {
        DEBUG_ERROR_OUT("Error: singleNodeTask failed; nodeid: " << cur_node.nodeid);
        return false;
    }

    // #4 非静态节点:非单点任务
    USHORT at_node = dyn_virt_status_.from_node_;
    bool latest_at_node = dyn_virt_status_.loc_type_ == TagStateType::STATIC_LOCATION;

    if (!latest_at_node) {
        DEBUG_OUT("current is not single node task; loctype is not static_location");
        return false;
    }

    // 如果当前节点等于上一次目标节点则是单点无效任务
    if (at_node == to_node) {
        DEBUG_ERROR_OUT("Error: current is single node task; atNode: " << at_node << ", toNode: " << to_node);
        return true;
    }

    DEBUG_OUT("current is not single node task; atNode: " << at_node << ", toNode: " << to_node);
    return false;
}

/**
 * @brief 编译订单任务
 *
 * 该函数用于编译新的订单任务，并根据任务内容更新相关状态。
 *
 * @return 编译成功返回true，否则返回false。
 */
bool TaskManager::OnlineCompileOrderTask()
{
    // 每个订单任务，读取设置一次当前订单的载货状态
    SetLoadFlags();

    // push front a new order
    agv_msgs::msg::Order order = order_task_buffer_.Get();
    DEBUG_OUT("OnlineCompileOrderTask: from order buffer: order:" << order);
    LogOutCurOrder(order);

    std::string order_id = order.orderid;
    uint32_t order_update_id = order.orderupdateid;
    std::string zone_set_id = order.zonesetid;

    // 判断节点的数量是否有效
    std::vector<agv_msgs::msg::MapNode> nodes = order.nodes;
    int node_count = nodes.size();

    if (node_count == 0) {
        OSS_TO_STRING(task_err_detail_, "Error: current order node is empty, orderid: " 
                        << order_id << ", orderupdateid: " << order_update_id);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    // 判断订单id是否有效
    long long order_id_num = 0;
    if (!string2Number(order_id, order_id_num)) {
        OSS_TO_STRING(task_err_detail_, "Error: current order id is not number, orderid: " 
                        << order_id << ", orderupdateid: " << order_update_id);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    // 判断是否是单点无效的空任务
    if (SingleNodeTask(order)) {
        OSS_TO_STRING(task_err_detail_, "Error: current order is SingleNodeCode");
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    // 更新最后工位点的word字段属性
    ZoneNodeWords(zone_set_id);

    // 更新动作状态ID 订单ID+当前订单子ID
    UpdateActionStateId(order_id_num, order_update_id);

    // 每次编译一次新的订单子任务都需要更新此状态
    task_status_ = START_WORKCODE;

    // compile current order task
    for (int i = 0; i < node_count; ++i) {
        agv_msgs::msg::MapNode cur_node = nodes[i];

        // 判断是否需要到区域目标点自旋到指定节点角度
        bool zone_spin = (i == (node_count - 1)) ? ZoneNodeSpin(zone_set_id, cur_node) : false;

         // 是否需要插入节点栈板识别动作
        bool is_pallet_identify = false;

        if (nodes[node_count - 1].actions.size() > 0) {
            if (nodes[node_count - 1].actions[0].actiontype == ACTION_PICK) {
                is_pallet_identify = i == (node_count - 2) ? IdentifyNode(zone_set_id, cur_node, nodes[node_count - 1]) : false;
            }
        }

        if (is_pallet_identify) {
            DEBUG_OUT("insert pallet identify");
        }

        // 设置一次工位点到点自旋属性
        ZoneSpinData zone_spin_data(zone_spin, cur_node);

        // 如果是当前节点和上一次点相同，也返回TRUE
        bool node_ret = OnlineCompileNavNodeCode(zone_spin_data);

        if (!node_ret) {
            // 具体错误已经在task_err_detail_中
            DEBUG_OUT("Error: compile current nodes failed, orderid: "
                    << order_id << ", orderupdateid: " << order_update_id
                    << ", nodeRet: " << node_ret);
            return false;
        }

        // 如果是当前节点不带任务，也返回TRUE
        bool action_ret = OnlineCompileActionCode(cur_node.actions);

        if (!action_ret) {
            // 具体错误已经在task_err_detail_中
            DEBUG_OUT("Error: compile current action failed, orderid: "
                    << order_id << ", orderupdateid: " << order_update_id
                    << ", actionRet: " << action_ret);
            return false;
        }

        // add 2024-8-15
        // 插入节点栈板识别动作
        if (is_pallet_identify) {
            bool ok = InsertIdentifyAction(cur_node.nodeid, zone_set_id);
            if (!ok) {
                // 具体错误已经在m_taskErrorDetails中
                DEBUG_OUT("error:InsertIdentifyAction failed,orderid:"
                          << order_id << ",orderupdateid:" << order_update_id
                          << ",cur_node.nodeid=" << cur_node.nodeid << ",zonesetid=" << zone_set_id);
                return false;
            }
        }

        if (pause_cur_compile_) {
            DEBUG_WARN_OUT("OnlineCompileOrderTask: force PauseCurCompile ignore remain task! return; i=" 
                            << i << ", node_count=" << node_count);
            break;
        }
    }

    // 如果准备设置当前的work标识，无work存在，说明编译过程出现了无效任务
    // 弹出最近任务ID，并更新当前任务ID
    if (!SetWorkBufferEndFlag()) {
        UpdateCurTaskId();
        DEBUG_OUT("SetWorkBufferEndFlag failed: tail workcode does not exist; pop up current task id and update current id");
    }

    return true;
}

/**
 * @brief 设置工作缓冲区结束标志
 *
 * 设置工作缓冲区中当前任务的结束标志，并更新任务状态。
 *
 * @return 如果设置成功返回 true，否则返回 false。
 */
bool TaskManager::SetWorkBufferEndFlag()
{
    USHORT pos = work_buffer_.TailPos();
    pos = work_buffer_.LastEntry(pos);
    WorkCode* work_code = work_buffer_.PeekItem(pos);
    
    if (work_code == nullptr) {
        DEBUG_OUT("Abort: Error! compile work code is null");
        return false;
    }
    
    work_code->task_flag_ |= END_WORKCODE;
    task_status_ = START_WORKCODE;
    can_compile_one_new_task_ = true;
    
    int type = work_code ? static_cast<int>(work_code->type_) : -9999;
    DEBUG_OUT("SetWorkBufferEndFlag work_code->type: " << type);
    
    return true;
}

/**
 * @brief 编译导航节点代码
 *
 * 根据传入的区域旋转数据编译导航节点代码。
 *
 * @param zone_spin_data 区域旋转数据
 * @return 编译成功返回true，失败返回false
 */
bool TaskManager::OnlineCompileNavNodeCode(ZoneSpinData zone_spin_data) {
    std::string node_id = zone_spin_data.nav_node_.nodeid;
    int to_node = 0;

    if (!string2Number(node_id, to_node)) {
        OSS_TO_STRING(task_err_detail_, "Error: CompileNavNodeCode failed; nodeid: " << node_id);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    bool latest_at_node = dyn_virt_status_.loc_type_ == TagStateType::STATIC_LOCATION;

    // 正常路径任务
    if (latest_at_node) {
        return CompileNodeToNodeTraj(to_node, zone_spin_data);
    } else {
        return CompileMidToNodeTraj(to_node, zone_spin_data);
    }

    return true;
}

/**
 * @brief 在线编译并执行动作代码
 *
 * 根据传入的动作向量，依次编译并执行每个动作。
 *
 * @param actions 动作向量，包含多个agv_msgs::msg::Action类型的元素
 * @return 若所有动作编译并执行成功，则返回true；否则返回false
 */
bool TaskManager::OnlineCompileActionCode(std::vector<agv_msgs::msg::Action> actions) 
{
    for (auto& action : actions) {
        std::string block_type = action.blockingtype;
        if (action.actionid.empty() || action.actiontype.empty()) {
            DEBUG_OUT("None action: actionid: " << action.actionid
                << ", actiontype: " << action.actiontype);
            break;
        }

        if (block_type == OP_MUTEX_TYPE) {
            if (dyn_virt_status_.loc_type_ != TagStateType::STATIC_LOCATION) {
                OSS_TO_STRING(task_err_detail_,
                                "Compile action op failed; current location is not node");
                DEBUG_ERROR_OUT(task_err_detail_);
                return false;
            }
            Node* op_node = world.GetNode(dyn_virt_status_.from_node_);

            if (!op_node) {
                OSS_TO_STRING(task_err_detail_,
                                "Error: current action node is null; virtulFromNode="
                                    << dyn_virt_status_.from_node_);
                DEBUG_ERROR_OUT(task_err_detail_);
                return false;
            }
            Node& action_node = *op_node;
            bool enable_load_center = action_node.EnableLoadCentering();
            // 增加二维码地码校验和是否开启
            int enable_qr_id = action_node.EnableMarkCode();
            int qr_id = action_node.GetQrId();
            agv_msgs::msg::ActionParam qr_id_param, mark_code_enable_param;
            qr_id_param.key = NAME_KEY_ACTION_QR_ID;
            qr_id_param.value = std::to_string(qr_id);
            action.actionparameters.push_back(qr_id_param);
            // 是否开启
            mark_code_enable_param.key = NAME_KEY_ACTION_ENABLE_QR_ID;
            mark_code_enable_param.value = std::to_string(enable_qr_id);
            action.actionparameters.push_back(mark_code_enable_param);
            DEBUG_OUT("action.actiontype = " << action.actiontype
                                            << ", enableLoadCenter: " << enable_load_center
                                            << ", qrIdParam.key: " << qr_id_param.key
                                            << ", qrIdParam.value: " << qr_id_param.value
                                            << ", markCodeEnableParam.key: " << mark_code_enable_param.key
                                            << ", markCodeEnableParam.value: " << mark_code_enable_param.value);
            Operation* cur_op = nullptr;
            if (action.actiontype == CHARGING_ACTION_TYPE) {
                cur_op = new OperationCharge(action);
            } else if (action.actiontype == ACTION_PICK && enable_load_center) {
                if (!IsOmniDirection()) {
                    cur_op = new OperationTurnAroundOnce(action);
                    DEBUG_OUT("Create COperationTurnAroundOnce obj");
                } else {
                    cur_op = new OperationTurnAroundOmni(action);
                    DEBUG_OUT("Create COperationTurnAroundOmni obj");
                }
                SetPauseCompileFlag(true);
            } else if (action.actiontype == WAIT_ACTION_TYPE) {
                cur_op = new OperationWaitForRelease(action);
            } else if (action.actiontype == CROSS_FLOOR_ACTION_TYPE) {
                cur_op = new OperationCrossFloor(action);
            } else {
                cur_op = new OperationPeripheral(action);
            }
            WorkCode work_code(WorkCodeType::OP_WORKCODE, reinterpret_cast<DWORD>(cur_op), task_status_);
            work_buffer_.Put(work_code, true);
            task_status_ = NORMAL_WORKCODE;
            dyn_virt_status_.last_is_op_ = true;
            DEBUG_OUT("Compile action op param: actiontype: " << action.actiontype
                                                            << ", actionid: " << action.actionid);
        } else if (block_type == DYN_TYPE) {
            Operation* cur_dyn_op = new OperationPeripheral(action);
            WorkCode work_code(WorkCodeType::DYN_WORKCODE, reinterpret_cast<DWORD>(cur_dyn_op), task_status_);
            work_buffer_.Put(work_code, true);
            task_status_ = NORMAL_WORKCODE;
            DEBUG_OUT("Compile dyn action op param: actiontype: " << action.actiontype
                                                                << ", actionid: " << action.actionid);
        } else {
            OSS_TO_STRING(task_err_detail_,
                        "Compile current action code is unknown; blockType=" << block_type);
            DEBUG_ERROR_OUT(task_err_detail_);
            return false;
        }
    }
    return true;
}

/**
 * @brief 根据Posture更新虚拟静态位置
 *
 * 根据给定的Posture更新虚拟静态位置。
 * DynVirtStatus update important: After compiling, 
 * the virtual pose is updated on the static node
 * @param nv TrajNv对象指针，包含轨迹信息
 * @param to_node 目标节点ID
 *
 * @return 如果更新成功返回true，否则返回false
 */
bool TaskManager::UpdateVirtualStaticPoseByPst(TrajNv* nv, USHORT to_node) 
{
    if (nv == nullptr) return false;

    Angle heading = nv->GetTraj()->EndHeading();
    MoveDir wheel_move_dir = nv->GetTraj()->WheelMoveDir();
    Node* to_node_ptr = world.GetNode(to_node);
    if (!to_node_ptr) {
        DEBUG_ERROR_OUT("Error: updateVirtualStaticPoseByPst failed; toNode is null");
        return false;
    }
    Node nd_to = *to_node_ptr;
    Posture pst(nd_to.GetPntObject(), heading);
    dyn_virt_status_.wheel_move_dir_ = wheel_move_dir;
    return dyn_virt_status_.StaticLocation(to_node, pst);
}


/**
 * @brief 更新虚拟静态位置
 *
 * 根据给定的轨迹节点信息更新虚拟静态位置。
 * After compiling, update the dynamic site of virtual pose (in the node section)
 * @param nv 轨迹节点指针
 * @param to_node 目标节点索引
 * @return 更新成功返回true，否则返回false
 */
bool TaskManager::UpdateVirtualStaticPose(TrajNv* nv, USHORT to_node) {
  if (nv == nullptr) return false;

  Angle heading = nv->GetTraj()->EndHeading();
  MoveDir wheel_move_dir = nv->GetTraj()->WheelMoveDir();
  Angle steer_angle = nv->GetTraj()->EndSteerAngle();
  return dyn_virt_status_.StaticLocation(to_node, heading, wheel_move_dir, steer_angle, level_);
}

/**
 * @brief 更新虚拟动态姿态
 *
 * 更新虚拟动态姿态，根据给定轨迹节点和绝对进度，计算当前位置和方向。
 * After compiling, update the dynamic site of virtual pose (in the node section)
 * @param nv 轨迹节点指针
 * @param at_node 当前节点索引
 * @param to_node 目标节点索引
 * @param abs_progress 绝对进度（0.0到1.0之间）
 *
 * @return 如果更新成功返回true，否则返回false
 */
bool TaskManager::UpdateVirtDynPose(TrajNv* nv, USHORT at_node, USHORT to_node, float abs_progress) 
{
    if (nv == nullptr) return false;
    Angle heading = nv->GetTraj()->EndHeading();
    MoveDir wheel_move_dir = nv->GetTraj()->WheelMoveDir();
    Angle steer_angle = nv->GetTraj()->EndSteerAngle();
    return dyn_virt_status_.DynamicLocation(at_node, to_node, abs_progress, heading, wheel_move_dir, steer_angle, level_);
}

/**
 * @brief 编译节点到节点的轨迹
 *
 * 根据给定的目标节点和区域自旋数据，编译节点到节点的轨迹。
 * FIXME: very importan  for example 1 2 3 65534 3 4 5 65534
 * @param to_node 目标节点编号
 * @param zone_spin_data 区域自旋数据
 *
 * @return 如果编译成功返回true，否则返回false
 */
bool TaskManager::CompileNodeToNodeTraj(USHORT to_node, ZoneSpinData zone_spin_data) 
{
    USHORT current_node = dyn_virt_status_.from_node_;
    // 如果当前节点等于目标节点，任务取消，并尝试查看是否需要插入区域自旋动作
    if (current_node == to_node) {
        if (zone_spin_data.zone_spin_) {
            // 判断是否需要插入区域自旋动作
            bool ret = InsertSpinTurn(nullptr, zone_spin_data.GetAngle());
            DEBUG_OUT("current_node" << current_node << ", set zone_spin: insert ret:" << ret);
        }
        DEBUG_OUT("current_node is equal to to_node, so cancel this path and continue to next node; current_node" 
                    << current_node << ", to_node:" << to_node);
        return true;
    }

    // 节点到节点之间，自动生成新的路径
    Path* path = nullptr;  // 根据起始任务节点获取路径指针，然后创建轨迹对象
    if (spin_turn_dir_ == SMART_TURN_DIR) {  // always true
        path = world.GetPathPointer(current_node, to_node, dyn_virt_status_.cur_posture_.theta_);
    } else {
        path = world.GetPathPointer(current_node, to_node, dyn_virt_status_.cur_posture_.theta_);
    }

    if (path == nullptr) {
        OSS_TO_STRING(task_err_detail_, "error: compile node_to_node_traj: path is null;" << "current_node=" 
                        << current_node << ", to_node=" << to_node);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    // 获取目标节点对象
    Node* node_to = world.GetNode(to_node);
    if (!node_to) {
        OSS_TO_STRING(task_err_detail_, "error: compile node_to_node_traj: node_to is null;" << ", to_node=" << to_node);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }
    Node node_to_copy = *node_to;

    // 创建轨迹对象并放入工作缓冲区
    TrajNv* nv = CreateNvObj(path, node_to_copy, 0.0f, path->Size());
    if (!nv) {
        OSS_TO_STRING(task_err_detail_, "error: compile node_to_node_traj: nv is null; " << ", to_node=" << to_node);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    UpdateVirtualStaticPoseByPst(nv, to_node);

    if (zone_spin_data.zone_spin_) {  // 判断是否需要插入区域自旋动作
        bool ret = InsertSpinTurn(nullptr, zone_spin_data.GetAngle());
        DEBUG_OUT("to_node" << to_node << ", set zone_spin: insert ret:" << ret);
    }
    return true;
}

/**
 * @brief 编译中间节点到目标节点的轨迹
 *
 * 根据当前动态虚拟状态和目标节点，将中间节点编译到目标节点的轨迹。
 *
 * @param target_node 目标节点编号
 * @param zone_spin_data 区域旋转数据
 *
 * @return 编译成功返回true，否则返回false
 */
bool TaskManager::CompileMidToNodeTraj(USHORT target_node, ZoneSpinData zone_spin_data) 
{
    USHORT cur_from_node = dyn_virt_status_.from_node_;
    USHORT cur_to_node = dyn_virt_status_.to_node_;
    USHORT new_from_node = (cur_to_node == target_node) ? cur_from_node : cur_to_node;

    if (target_node != cur_from_node && target_node != cur_to_node) {
        OSS_TO_STRING(task_err_detail_, "error: new_to_node cannot match current path; CompileMidToNodeTraj: "
                                        << "target_node=" << target_node
                                        << ", cur_from_node=" << cur_from_node << ", cur_to_node=" << cur_to_node);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    Path* path;
    if (spin_turn_dir_ == SMART_TURN_DIR) {
        path = world.GetPathPointer(new_from_node, target_node, dyn_virt_status_.cur_posture_.theta_);
    } else {
        path = world.GetPathPointer(new_from_node, target_node, dyn_virt_status_.cur_posture_.theta_);
    }

    if (path == nullptr) {
        OSS_TO_STRING(task_err_detail_, "error: compile mid_to_node_traj: path is null; "
                                        << "target_node:" << target_node
                                        << ", cur_from_node=" << cur_from_node << ", cur_to_node=" << cur_to_node
                                        << "zone_spin:" << zone_spin_data.zone_spin_);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    float from_progress = dyn_virt_status_.abs_progress_;
    if (new_from_node == cur_to_node) {
        DEBUG_OUT("CompileMidToNodeTraj: use new_from_progress=" << (path->Size() - from_progress)
                                                                    << ", total_path_size=" << path->Size()
                                                                    << ", old_from_progress=" << from_progress);
        from_progress = path->Size() - from_progress;
    }
    float to_progress = path->Size();

    Node* node_to = world.GetNode(target_node);
    if (!node_to) {
        OSS_TO_STRING(task_err_detail_, "error: compile mid_to_node_traj: node_to is null; target_node=" << target_node);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }
    Node node_to_copy = *node_to;

    TrajNv* nv = CreateNvObj(path, node_to_copy, from_progress, to_progress);
    if (nv == nullptr) {
        OSS_TO_STRING(task_err_detail_, "error: compile mid_to_node_traj: nv is null; target_node=" << target_node);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }
    UpdateVirtualStaticPose(nv, target_node);

    if (zone_spin_data.zone_spin_) {
        bool ret = InsertSpinTurn(nullptr, zone_spin_data.GetAngle());
        DEBUG_OUT("target_node" << target_node << ", set zone_spin: insert ret:" << ret);
    }
    return true;
}

/**
 * @brief 编译指定节点的中间轨迹
 *
 * 将从当前动态虚拟状态中的起始节点到目标节点的路径编译为中间轨迹。
 * (USHORT * 4): 0xFEF4 toNode integer decimal*1000
 * @param to_node 目标节点的节点编号
 *
 * @return 如果编译成功，则返回true；否则返回false
 */
bool TaskManager::CompileNodeToMidTraj(USHORT to_node) {
    USHORT at_node = dyn_virt_status_.from_node_;
    Path* path = world.GetPathPointer(at_node, to_node);
    if (path == nullptr) return false;

    Node* node_to = world.GetNode(to_node);
    if (!node_to) {
        DEBUG_ERROR_OUT("compile_node_to_mid_traj node_to is null" << to_node);
        return false;
    }
    Node node_to_copy = *node_to;

    float target_abs_progress = GetTargetProgress();

    TrajNv* nv = CreateNvObj(path, node_to_copy, 0.0f, target_abs_progress);
    if (nv == nullptr) return false;

    UpdateVirtDynPose(nv, at_node, to_node, target_abs_progress);
    return true;
}

/**
 * @brief 编译中间轨迹
 *
 * 将当前动态虚拟状态中的路径编译成中间轨迹。
 *
 * @param to_node 目标节点编号
 *
 * @return 如果编译成功返回true，否则返回false
 */
bool TaskManager::CompileMidToMidTraj(USHORT to_node) {
    USHORT node1 = dyn_virt_status_.from_node_;
    USHORT node2 = dyn_virt_status_.to_node_;
    USHORT from_node = (node2 == to_node) ? node1 : node2;

    if (to_node != node1 && to_node != node2) {
        DEBUG_ERROR_OUT("error: new_to_node cannot match current path; CompileMidToMidTraj: new_to_node=" << to_node
                                                                                                            << ", cur_from_node=" << node1
                                                                                                            << ", cur_to_node=" << node2);
        return false;
    }

    Path* path = world.GetPathPointer(from_node, to_node);
    if (path == nullptr) {
        DEBUG_ERROR_OUT("error: CompileMidToMidTraj current path is null; from_node=" << from_node
                                                                                        << ", to_node" << to_node);
        return false;
    }

    float from_progress = dyn_virt_status_.abs_progress_;
    if (from_node == node2) {
        DEBUG_OUT("CompileMidToMidTraj: use new_from_progress=" << (path->Size() - from_progress)
                                                                << ", total_path_size=" << path->Size()
                                                                << ", old_from_progress=" << from_progress);
        from_progress = path->Size() - from_progress;
    }
    float to_progress = GetTargetProgress();

    Node* node_to = world.GetNode(to_node);
    if (!node_to) {
        DEBUG_ERROR_OUT("error: CompileMidToMidTraj node_to is null; to_node=" << to_node);
        return false;
    }
    Node node_to_copy = *node_to;

    TrajNv* nv = CreateNvObj(path, node_to_copy, from_progress, to_progress);
    if (nv == nullptr) return false;

    UpdateVirtDynPose(nv, from_node, to_node, to_progress);
    return true;
}
//
//   Get the target progress for the current navigation code.
//
float TaskManager::GetTargetProgress()
{
    // float fDigit = (float)m_TaskBuffer.Get(); // Integer(ushort) = 10 and  decimal(ushort) = 1  10.001
    // float fFraction = (float)m_TaskBuffer.Get();
    // return (fDigit + fFraction / 1000.0f);
    return -1.0f;
}

/**
 * @brief 在任务管理器中创建一个新的TrajNv对象
 *
 * 根据给定的路径、目标节点、起始速度和终止速度创建一个新的TrajNv对象。
 * 如果航向角不适合并且不在节点或旋转点上，则会删除该对象。
 * 如果不需要停止，则修正一次的速度，并可能会覆盖循环检测修正。
 * 如果需要停止，则设置相应的标志。
 * 最后，将创建的对象添加到工作代码队列中。
 *
 * @param path 路径对象指针
 * @param to_node 目标节点对象引用
 * @param from 起始速度
 * @param to 终止速度
 * @param force_check_make 是否强制检查并创建对象
 *
 * @return 创建的TrajNv对象指针，如果创建失败则返回nullptr
 */
TrajNv* TaskManager::CreateNvObj(Path* path, Node& to_node, float from, float to, bool force_check_make) 
{
    DEBUG_OUT("Ready to create new nv obj, node to: " << to_node.id_);
    TrajNv* compile_nv = new TrajNv(path, to_node, from, to, level_, force_check_make, use_speed_level_);
    if (!compile_nv) {
        DEBUG_ERROR_OUT("Error: createNvObj compileNv is null");
        return nullptr;
    }

    if (!compile_nv->GetTraj()) {
        DEBUG_ERROR_OUT("Error: createNvObj compileNv->getTraj is null");
        return nullptr;
    }

    // 如果航向角不适合并且不在节点或旋转点上，则删除mbnv
    if (!HeadingFit(compile_nv)) {
        if (dyn_virt_status_.loc_type_ != TagStateType::STATIC_LOCATION || !InsertSpinTurn(compile_nv)) {
            if (compile_nv) {
                delete compile_nv;
                compile_nv = nullptr;
            }
            DEBUG_OUT("Head angle no match: cannot insert spin turn traj");
            return nullptr;
        }
    }

    if (!NeedStop(compile_nv)) { // 4 condition (HeadingFit)
        // 此时修正一次的速度，可能会被循环检测修正覆盖
        compile_nv->last_nv_need_stop_ = false;
        float next_cruise_vel = compile_nv->LinearCruiseVel();
        WorkCode* tail_work_code = FindTailWorkCode();
        if (tail_work_code != nullptr) {
            TrajNv* tail_nv = (TrajNv*)(tail_work_code->pointer_);
            ModifyEndVel(tail_nv, next_cruise_vel);
            DEBUG_OUT("#1 modify end vel =" << next_cruise_vel);
        } else if (cur_nv_ != nullptr) {
            if (!cur_nv_->OverrideEndVel()) {
                ModifyEndVel(cur_nv_, next_cruise_vel);
                DEBUG_OUT("#2 modify end vel =" << next_cruise_vel);
            }
        }
    } else {
    	// flag m_bLastNvNeedStop is usee for function( ComputeLastNvEndVel() )
        // steer、wheeldir、op; set stop；important
        compile_nv->last_nv_need_stop_ = true;
        DEBUG_OUT("Last Nv needs stop; next new compile nv; AGV needs to stop to fit");
    }
    
    // Append it to the "Work Code" queue
    WorkCode workcode(WorkCodeType::NV_WORKCODE, reinterpret_cast<DWORD>(compile_nv), task_status_);
    work_buffer_.Put(workcode);
    WorkCode workcodecopy(WorkCodeType::NV_WORKCODE, reinterpret_cast<DWORD>(compile_nv), task_status_);
    work_buffer_copy_.Put(workcodecopy);
    task_status_ = NORMAL_WORKCODE;
    return compile_nv;
}

// float TaskManager::GetActualRange(TrajNv *pCurNv)
// {
//     float fRange = 0.0f;
//     if (pCurNv == NULL)
//     {
//         return fRange;
//     }
//     // if (pCurNv->GetGuideType() == LASER_GUIDANCE || pCurNv->GetGuideType() == QR_CODE_GUIDANCE)
//     if (pCurNv->GetGuideType() != NO_GUIDANCE)
//     {
//         fRange = pCurNv->GetTraj()->GetRange(); // laser use real paht length
//     }
//     else
//     {
//         // FIXME:
//         fRange = pCurNv->GetTraj()->GetRange() - pCurNv->m_fSureArea / 2.0f;
//     }
//     return fRange;
// }

// void TaskManager::PathLengthForEv(TrajNv *pCurNv, float &fCurNvEndVel, float &fPathLength, bool &bIgnore, const float &fRange)
// {
//     CMapBasedNv *pCurNvTemp = (CMapBasedNv *)pCurNv;
//     CPath *pPath = pCurNvTemp->GetPath();
//     // USHORT uEndNode = pCurNvTemp->GetEndNode();
//     // Node *pEndNode = world.GetNode(uEndNode);
//     if (pPath != NULL /* && pEndNode != NULL*/)
//     {

//         // float fChkMarkDist = pEndNode->m_fChkMarkDist; //// FIXME: set 0.0f is problem
//         // fPathLength = fRange - fChkMarkDist;
//         // if (fPathLength <= 0.0f)
//         // {
//         //     fPathLength = fRange; // #1
//         // }
//         // else if (fChkMarkDist >= 0.1f && fPathLength <= 0.1f)
//         // {
//         //     bIgnore = TRUE; // #2  pathLengh too short: 0.05+0.45  Approximately equal to fChkMarkDist to set ignore
//         // }

//         // #3 FIXME: Change the end speed of the end point of the current section, and the default is zero
//         // 货物状态
//         bool load = CMotionParm::m_loadFlags.currentLoad;
//         float max_vel = load ? pPath->m_fVelLimitLoad : pPath->m_fVeloLimit;
//         fCurNvEndVel = __min_(fCurNvEndVel, max_vel);
//     }
// }

// void TaskManager::ModifyVelZeroForEv(TrajNv *pLastNv, float &fLastNvEndVel, bool &bStopped)
// {

//     fLastNvEndVel = 0.0f;
//     bStopped = TRUE;
//     ModifyEndVel(pLastNv, 0.0f); // for ComputeLastNvEndVel
// }
/**
 * @brief 计算工作缓冲区中最后一段的结束速度
 *
 * 该函数通过遍历工作缓冲区中的任务，计算最后一段的结束速度。
 *
 * @return 如果计算成功返回true，否则返回false。
 */
bool TaskManager::ComputeLastNvEndVel()
{
    // float fPathLength = 0.0f;
    float pre_max_end_vel = MotionParm::max_vel_;

    // work_buffer_Copy.TailPos();//tail's next cursor
    USHORT headpos = work_buffer_copy_.HeadPos();
    USHORT tailpos = work_buffer_copy_.LastEntry(work_buffer_copy_.TailPos());

    bool stopped = true;
    // float fCursorNvEndVel = 0.0f;
    // bool bIgnore = FALSE;

    // Calculate the speed from back to front until the head of the team jumps out
    // pCurNv is buffer's final one
    for (USHORT cursor = tailpos;;) {
        // tailpos to headpos jump loop
        if (cursor == headpos) break; // exit condition

        WorkCode* cursor_code = work_buffer_copy_.PeekItem(cursor);
        WorkCode* cursor_pre = nullptr;
        if (cursor_code->type_ == WorkCodeType::NV_WORKCODE) {
            // #1 Number one from the bottom at the tail of queue
            TrajNv* cursor_nv = (TrajNv*)(cursor_code->pointer_);
            if (!cursor_nv) {
                DEBUG_ERROR_OUT("error: ComputeLastNvEndVel pCursorNv is null");
                return false;
            }

            // #1 查看路段的实际长度
            // 设置路段长度 ：spin: 0 ;最后停车点 = Lengh-开放距离  非停车点 = Lengh总长度
            // const:pCursorNv、fActualRange
            // 引用：bStopped(只有最末端是true,使用过即改为false)、fCursorNvEndVel(游标的结束速度)、fPathLength(计算使用时的有效长度)、bIgnore(路段太短是否可忽略)
            // 修改简化后，只有自旋是0 其他是实际长度
            // fPathLength = CalcPathLength(bStopped, pCursorNv, fCursorNvEndVel, fPathLength, bIgnore);

            // #2 根据路径长度和减速度 计算路段起始点的最大速度  V2^2-V1^2=2aS
            // 传入参数皆是拷贝，非引用，只使用返回值 fPreMaxEndVel
            // fPreMaxEndVel = CalcPreMaxEndVel(pCursorNv, fPreMaxEndVel, fCursorNvEndVel, fPathLength, bIgnore);
            // 使用倒数第一段的巡航速度作为倒数第二段的终点速度
            TrajNv* cursor_nv_temp = cursor_nv;
            Path* path = cursor_nv_temp->GetPath();
            if (path) {
                // #3 FIXME: Change the end speed of the end point of the current section, and the default is zero
                // 货物状态
                bool load = MotionParm::load_flags_.current_load;
                pre_max_end_vel = load ? path->velocity_limit_load_ : path->velocity_limit_;
            }

            // #3 Number two from the bottom at the tail of queue
            cursor_pre = work_buffer_copy_.PeekItem(work_buffer_copy_.LastEntry(cursor));
            TrajNv *cursor_pre_nv = (TrajNv*)(cursor_pre->pointer_);
            ModifyCursorPreEndVel(cursor_pre_nv, cursor_nv, stopped, pre_max_end_vel); // important
        }

        // #3 Ring queue  if the tail is in front of the head jump 0 to end
        if (cursor == 0) { 
            cursor = work_buffer_copy_.GetSize() - 1; 
        } else  { 
            cursor--; 
        }
        // important iteration
        // fCursorNvEndVel = fPreMaxEndVel;
    }   // while(1);end
    return true;
}

/**
 * @brief 判断是否停止
 *
 * 根据给定的前一个和最后一个轨迹类型，判断是否可以设置停止。
 *
 * @param pre_nv_type 前一个轨迹类型
 * @param tail_nv_type 最后一个轨迹类型
 *
 * @return 如果可以设置停止，则返回true；否则返回false
 */
bool TaskManager::SetLastNvStop(SHORT pre_nv_type, SHORT tail_nv_type) 
{
  bool set = false;
  short pre = pre_nv_type;
  short tail = tail_nv_type;
  bool state1 = pre == static_cast<short>(TrajType::LINE_TRAJ) &&
                tail == static_cast<short>(TrajType::SIDE_TRAJ);  // line to side
  bool state2 = pre == static_cast<short>(TrajType::SIDE_TRAJ) &&
                tail == static_cast<short>(TrajType::LINE_TRAJ);  // side to line
  bool state = state1 || state2;
  if (state) {
    set = true;
  }
  return set;
}

//
// 查看路段的实际长度
// 设置路段长度 ：spin: 0 ;最后停车点 = Lengh-开放距离  非停车点 = Lengh总长度
//

// float TaskManager::CalcPathLength(bool &bStopped, TrajNv *pCursorNv, float &fCursorNvEndVel, float &fPathLength, bool &bIgnore)
// {

//     if (!pCursorNv)
//     {
//         DEBUG_OUT("error:CalcPathLength:pCursorNv is null ");
//         return 0.0f;
//     }
//     // 倒数一个路段长度
//     fPathLength = pCursorNv->GetTraj()->GetRange();
//     SHORT uCursorTrajType = pCursorNv->GetTrajType();
//     // bool bProfileZero = fabs(pCursorNv->profiler_.GetEndValue()) <= 0.0001f; // read
//     // bool bLineTraj = LINE_TRAJ == uCursorTrajType;

//     if (SPIN_TRAJ == uCursorTrajType)
//     {
//         if (bStopped)
//         {
//             DEBUG_OUT("CalcPathLength:pCursorNv is spin; need stopped,clear once flag ");
//             bStopped = FALSE;
//         }
//         fPathLength = 0.0f;
//         return fPathLength;
//     }

//     if (bStopped)
//     {
//         bStopped = FALSE;
//         DEBUG_OUT("CalcPathLength:pCursorNv need stopped,clear once flag ");

//         // if (bLineTraj && bProfileZero)
//         // {
//         //     // Line stop ; fCursorNvEndVel=0.1
//         //     // other detault is fCursorNvEndVel=0
//         //     // delete
//         //     //区分载货和无货速度
//         //     PathLengthForEv(pCursorNv, fCursorNvEndVel, fPathLength, bIgnore, fRange);
//         // }
//     }

//     return fPathLength;
// }

/**
 * @brief 计算预期最大结束速度
 *
 * 根据给定的路径长度、当前速度、预期最大结束速度以及是否忽略路径长度小于等于0.1的条件，
 * 计算并返回预期的最大结束速度。
 *
 * @param cursor_nv 指向TrajNv结构体的指针，包含当前节点信息
 * @param pre_max_end_velocity 预期的最大结束速度
 * @param cursor_nv_end_velocity 当前节点的结束速度
 * @param path_length 路径长度
 * @param ignore 是否忽略路径长度小于等于0.1的条件
 *
 * @return 预期的最大结束速度
 */
float TaskManager::CalcPreMaxEndVel(TrajNv* cursor_nv, float pre_max_end_velocity, float cursor_nv_end_velocity, float path_length, bool ignore)
{
    if (!cursor_nv) {
        DEBUG_OUT("error: CalcPreMaxEndVel - cursor_nv is null");
        return 0.0f;
    }

    float deceleration  = -MotionParm::down_acc_;
    // 如果路径长度小于等于0.1且不忽略，则将预期最大结束速度设置为0.05
    if (path_length <= 0.1f && !ignore) {
        pre_max_end_velocity = 0.05f;
    } else {
        // 根据公式 V2^2 = V1^2 + 2aS 计算预期最大结束速度
        pre_max_end_velocity = sqrt(fabsf((cursor_nv_end_velocity * cursor_nv_end_velocity) + 
                                fabsf(2.0f * deceleration * path_length)));
    }
    // If the same route, the same deceleration, deceleration to the end, the speed is too high, limit it. (the deceleration distance is not enough)）
    if (pre_max_end_velocity > cursor_nv->GetLinearVel()) {
        pre_max_end_velocity = cursor_nv->GetLinearVel();
    }

    return pre_max_end_velocity;
}

/**
 * @brief 修改前一个轨迹点的结束速度
 *
 * 根据当前轨迹点和前一个轨迹点的信息，决定是否修改前一个轨迹点的结束速度。
 *
 * @param cursor_pre_nv 前一个轨迹点指针
 * @param cursor_nv 当前轨迹点指针
 * @param stopped 是否停止的标志位
 * @param pre_max_end_vel 前一个轨迹点的最大结束速度
 */
void TaskManager::ModifyCursorPreEndVel(TrajNv* cursor_pre_nv, TrajNv* cursor_nv, bool& stopped, float& pre_max_end_vel)
{
    if (!cursor_pre_nv || !cursor_nv) {
        DEBUG_OUT("Error: ModifyCursorPreEndVel failed");
        return;
    }

    short pre_traj_type = cursor_pre_nv->GetTrajType();
    short cursor_traj_type = cursor_nv->GetTrajType();

    bool equal_move_dir = cursor_nv->GetMoveDir() == cursor_pre_nv->GetMoveDir();
    bool pre_spin = static_cast<short>(TrajType::SPIN_TRAJ) == pre_traj_type;

    // 上一次不是自旋 || 当前是自旋 || 上一次需要停车 || 轮转向变化
    bool modify_zero = cursor_traj_type == static_cast<short>(TrajType::SPIN_TRAJ) || 
                                            cursor_nv->last_nv_need_stop_ || 
                                            !equal_move_dir;
    if (pre_spin || modify_zero || SetLastNvStop(pre_traj_type, cursor_traj_type)) {
        if (!cursor_pre_nv->OverrideEndVel()) {
            // Output param: cursor_pre_nv, pre_max_end_vel(0.0f), stopped(true)
            pre_max_end_vel = 0.0f;
            stopped = true;
            ModifyEndVel(cursor_pre_nv, pre_max_end_vel); // For ComputeLastNvEndVel
        }
    } else {
        // Min (lastEndVel and lineCruiseVel)
        // #2 取当前和上一次最小值
        float cursor_pre_cruise_vel = cursor_pre_nv->LinearCruiseVel();
        if (!cursor_pre_nv->OverrideEndVel()) {
            pre_max_end_vel = pre_max_end_vel < cursor_pre_cruise_vel ? pre_max_end_vel : cursor_pre_cruise_vel;
            ModifyEndVel(cursor_pre_nv, pre_max_end_vel); // For ComputeLastNvEndVel
        }
    }

    DEBUG_OUT("PreNv start_id=" << cursor_pre_nv->start_node_id_ 
                << ", end_id=" << cursor_pre_nv->end_node_id_ 
                << ", endVel=" << cursor_pre_nv->GetEndValue());
    DEBUG_OUT("CursorNv start_id=" << cursor_nv->start_node_id_ 
                << ", end_id=" << cursor_nv->end_node_id_ 
                << ", endVel=" << cursor_nv->GetEndValue());
}

/**
 * @brief 向任务管理器中插入旋转轨迹
 *
 * 该函数用于向任务管理器中插入一个旋转轨迹。
 *
 * @param compile_nv 编译导航节点指针，如果为nullptr，则使用adjust_spin_angle作为旋转角度
 * @param adjust_spin_angle 调整旋转角度，仅在compile_nv为nullptr时使用
 *
 * @return 如果成功插入旋转轨迹，则返回true；否则返回false
 */
bool TaskManager::InsertSpinTurn(TrajNv* compile_nv, float adjust_spin_angle) 
{
    Node* spin_node = world.GetNode(dyn_virt_status_.from_node_);  // FIXME: change from_node_ to to_node_
    if (!spin_node) {
        DEBUG_ERROR_OUT("error: insert spin failed; spin_node is null");
        return false;
    }
    // 节点是否允许旋转
    Node& spin_nd = *spin_node;  // FIXME: change from_node_ to to_node_
    bool enable_spin = spin_nd.EnableSpin();
    if (!enable_spin) {
        DEBUG_ERROR_OUT("error: insert spin failed; map attribute cannot set current node to spin");
        return false;
    }
    Angle start_angle = dyn_virt_status_.cur_posture_.GetAngle();
    Angle end_angle = compile_nv ? compile_nv->GetTraj()->StartHeading() : adjust_spin_angle;
    bool sync_type = false;
    DEBUG_OUT("spin node=" << spin_nd.id_
                           << ", start_angle1=" << start_angle.radian_
                           << ", end_angle2=" << end_angle.radian_
                           << ", is end task node spin:" << (compile_nv == nullptr));
    TrajNv* nv_spin = new TrajNv(&spin_nd, start_angle, end_angle, level_, sync_type, spin_turn_dir_, use_speed_level_);
    if (!nv_spin) return false;
    WorkCode work_code(WorkCodeType::NV_WORKCODE, reinterpret_cast<DWORD>(nv_spin), task_status_);
    work_buffer_.Put(work_code);
    WorkCode work_code_copy(WorkCodeType::NV_WORKCODE, reinterpret_cast<DWORD>(nv_spin), task_status_);
    work_buffer_copy_.Put(work_code_copy);
    task_status_ = NORMAL_WORKCODE;
    // Update current compilation state
    UpdateVirtualStaticPose(nv_spin, spin_nd.id_);
    return true;
}

/**
 * @brief 判断当前导航是否需要停止
 *
 * 根据给定的TrajNv指针，判断是否需要停止。
 *
 * @param nv TrajNv指针，指向轨迹节点
 * @return 如果需要停止任务，则返回true；否则返回false
 */
bool TaskManager::NeedStop(TrajNv* nv) 
{
    bool should_stop = false;
    // 1
    if (!SteerAngleFit(nv)) {
        should_stop = true;
        DEBUG_OUT("agv to stop for steerAngleFit");
    }
    // 2
    if (!WheelMoveDirFit(nv)) {
        should_stop = true;
        DEBUG_OUT("agv to stop for wheelMoveDirFit");
    }
    // 3
    if (dyn_virt_status_.last_is_op_) {
        dyn_virt_status_.last_is_op_ = false;
        should_stop = true;
        DEBUG_OUT("agv to stop for Op");
    }
    return should_stop;
}

/**
 * @brief 修改轨迹的结束速度
 *
 * 该函数用于修改轨迹的结束速度。如果传入的轨迹指针不为空，则调用轨迹对象的ModifyEndValue函数来修改结束速度。
 *
 * @param nv 轨迹对象的指针
 * @param end_vel 新的结束速度值
 */
void TaskManager::ModifyEndVel(TrajNv* nv, float end_vel) 
{
    if (nv != nullptr) {
        nv->ModifyEndValue(end_vel);
    }
}

/**
 * @brief 更新当前任务ID
 *
 * 此函数用于更新当前任务的ID及其子任务ID，并设置任务状态。
 *
 * 如果任务ID缓冲区为空，则将VhcStatus中的任务ID、子任务ID和任务状态设置为默认值，并输出调试信息。
 * 如果任务ID缓冲区不为空，则从缓冲区中弹出一个TaskId对象，并将其任务ID和子任务ID赋值给VhcStatus，同时将任务状态设置为TaskStatus::Start。
 * 调用UpdateActionStateId函数更新操作状态ID。
 * 更新当前任务ID和子任务ID，并输出调试信息。
 */
void TaskManager::UpdateCurTaskId() 
{
  if (task_id_buffer_.IsEmpty()) {
    VhcStatus.task_id_ = 65535;
    VhcStatus.sub_task_id_ = 65535;
    VhcStatus.task_status_ = TaskStatus::UnKnown;
    DEBUG_OUT("UpdateCurTaskId task id is empty");
  } else {
    TaskId task_id = task_id_buffer_.Get();  // pop up
    VhcStatus.task_id_ = task_id.task_id;
    VhcStatus.sub_task_id_ = task_id.subtask_id;
    VhcStatus.task_status_ = TaskStatus::Start;
  }
  UpdateActionStateId(VhcStatus.task_id_, VhcStatus.sub_task_id_);
  cur_task_id_ = VhcStatus.task_id_;
  cur_sub_task_id_ = VhcStatus.sub_task_id_;
  DEBUG_OUT("curWork code is StartOfTask: curTaskID:" << cur_task_id_
                                                      << ",curSubTaskID:" << cur_sub_task_id_
                                                      << ",idBuffer.Count=" << static_cast<int>(task_id_buffer_.GetCount()));
}

/**
 * @brief 强制清除当前任务ID
 *
 * 该函数用于强制清除当前任务ID及其相关状态信息，将任务ID和子任务ID重置为65535，并将任务状态设置为未知状态。
 *
 * @note 此函数会调用 DEBUG_OUT 输出调试信息。
 */
void TaskManager::ForceClearCurTaskId()
{
    DEBUG_OUT("ForceClearCurTaskId start ");
    task_id_buffer_.RemoveAll(); // 重复清除
    VhcStatus.task_id_ = 65535;
    VhcStatus.sub_task_id_ = 65535;
    VhcStatus.task_status_ = TaskStatus::UnKnown;
    cur_task_id_ = VhcStatus.task_id_;
    cur_sub_task_id_ = VhcStatus.sub_task_id_;
    DEBUG_OUT("ForceClearCurTaskId: curTaskID:" << cur_task_id_ << ",curSubTaskID:" << cur_sub_task_id_
                                                << ",idBuffer.Count=" << (int)task_id_buffer_.GetCount());
}

/**
 * @brief 判断侧移轨迹是否可以反向
 *
 * 调用CanSteerReserve方法，以判断是否可以进行反向。
 *
 * @param traj 轨迹对象指针
 * @return 如果可以，返回true；否则返回false
 */
bool TaskManager::SideCanSteerReserve(Trajectory* traj)
{
    if (!traj) {
        DEBUG_OUT("error:SideCanSteerReserve is null traj");
        return false;
    }

    if (traj->GetType() == static_cast<short>(TrajType::SIDE_TRAJ)) {
        auto* side_traj = dynamic_cast<SideTrajectory*>(traj);
        if (!side_traj) {
            DEBUG_ERROR_OUT("SideCanSteerReserve:cur side traj is null");
            return false;
        }
        return side_traj->CanSteerReserve();
    } else {
        return false;
    }
}

/**
 * @brief 检查是否需要进行轨迹重定位
 *
 * 该函数用于检查是否需要进行轨迹重定位，并根据检查结果更新重定位状态。
 *
 * @details
 * - 首先检查是否已经找到下一个路段或重定位已完成，如果是，则直接返回。
 * - 然后检查是否需要根据路径位进行强制重定位检测，如果是，则设置找到下一个路段，并将重定位阶段设置为等待重定位，触发停车信号，并设置停车类别为重定位停车。
 * - 如果不需要根据路径位进行强制重定位检测，则检查下一个路径是否存在且需要重定位。
 *   - 如果存在且需要重定位，则设置找到下一个路段，并将重定位阶段设置为等待重定位，触发停车信号，并设置停车类别为重定位停车。
 *   - 如果不需要重定位，则将重定位阶段设置为无需重定位。
 */
void TaskManager::CheckTrajForRelocate()
{
    // 下一个路段找到和重定位完成
    if (relocate_.NextTrajFind() || relocate_.GetStage() == RelocateStage::RelocateFinish)  return; 

    // #1 如果设置了强制重定位检测检测标识且需要重定位则直接设置
    // 手动设置检测
    bool need = NeedRelocateByPathBits();
    if (need) {
        // 假装检测到了下一路径
        relocate_.SetNextTrajFind(true);
        relocate_.SetStage(RelocateStage::RelocateWait);
        // 触发停车信号
        relocate_.RelocateTrigOnce();
        SetStopCategory(StopType::STOP_RELOCATE, true);
        DEBUG_OUT("NeedRelocateByPathBits relocateTrigOnce");
        return;
    }

    // #2 检测是否需要重定位-路段并没有设置bit15
    // 自动检测
    // 如果一直无下一个路径，则标识设置未完成检测
    bool relocate = false;
    // 如果下一个路径存在且需要重定位
    if (HasNextReadyTraj(relocate)) {
        // 设置找到了下一个路径，当前轨迹，不再继续轮训-设置标识标记
        relocate_.SetNextTrajFind(true);
        relocate_.SetStage(relocate ? RelocateStage::RelocateWait : RelocateStage::RelocateNo);
        if (relocate)
        {
            // 触发停车信号
            relocate_.RelocateTrigOnce();
            SetStopCategory(StopType::STOP_RELOCATE, true);
            DEBUG_OUT("relocateTrigOnce");
        }

        DEBUG_OUT("HasNextReadyTraj;set finish ok;relocate=" << relocate);
    }
}

/**
 * @brief 判断是否需要根据路径BIT位重新定位
 *
 * 根据路径位信息判断是否需要重新定位位置
 *
 * @return 若需要重新定位，则返回true；否则返回false
 */
bool TaskManager::NeedRelocateByPathBits()
{
    bool need = false;
    RunPathProperty property = GetRunPathProperty();
    uint16_t bits = property.bits;

    // 注意：此处最大只支持检测16位
    // 如果路径属性设置该路段检测slam评分
    bool check = TestBit(bits, FORCE_RELOCATE_PATH_BITS);
    if (check) {
        // 检测评分
        float score = 0.0f;
        if (GetSlamScore(score)) {
            // 获取成功
            need = score < RELOACATE_SOCRE_LIMIT;
            DEBUG_OUT("RelocatePosByPathBits try to relocate? need=" << need << ",score=" << score);
        } else {
            // 数据超时，尝试重定位
            need = true;
            DEBUG_OUT("RelocatePosByPathBits try to relocate");
        }
    }
    return need;
}

/**
 * @brief 重新定位周期函数
 *
 * 此函数用于处理重新定位。
 *
 * @details 该函数首先调用 CheckTrajForRelocate 函数检查轨迹是否需要重新定位。
 * 如果找到下一条需要重新定位的轨迹，则进入等待状态，等待一段时间后释放停车信号。
 * 如果在等待期间完成重新定位，则设置重新定位状态为完成，并调用 SetStopCategory 函数释放停车信号。
 * 如果在等待期间未完成重新定位，则直接调用 SetStopCategory 函数释放停车信号。
 */
void TaskManager::RelocateCycle()
{
    CheckTrajForRelocate();
    if (!relocate_.NextTrajFind()) { return; }

    // 等待时间到，并释放停车信号
    if (relocate_.GetStage() == RelocateStage::RelocateWait) {
        bool wait_finish = relocate_.WaitRelocateFinish(8000);
        if (wait_finish) {
            // 释放停车信号
            relocate_.SetStage(RelocateStage::RelocateFinish);
            SetStopCategory(StopType::STOP_RELOCATE, false);
        }
        DEBUG_OUT("RelocateCycle waitFinish...finish=" << wait_finish);
    } else {
        // 释放停车信号
        SetStopCategory(StopType::STOP_RELOCATE, false);
    }
}

/**
 * @brief 判断是否存在下一个就绪的轨迹，并根据条件决定是否强制重定位
 *
 * 该函数检查是否存在下一个就绪的轨迹，并根据当前和下一个导航类型决定是否需要进行重定位。
 *
 * @param force_relocate 用于存储是否需要强制重定位的布尔引用
 * @return 如果存在下一个就绪的轨迹，则返回true；否则返回false
 */
bool TaskManager::HasNextReadyTraj(bool& force_relocate) 
{
    bool result = false;
    force_relocate = false;

    WorkCode* head_code = FindHeadNvWorkCode();
    // 如果没有下一个工作码(路径)，则等待下个工作(路径)码到来，标识不检测触发
    if (!head_code) {
        DEBUG_ERROR_OUT("CheckForceRelocate failed: head_code is null");
        return result;
    }

    // 当前是二维码模式，下一次路径时激光导航需要调整
    short current_guide_type = GetCurNavType();
    short next_guide_type = TrajGuideType(head_code);
    bool is_qr_or_mag = current_guide_type == QR_CODE_GUIDANCE || current_guide_type == MAG_GUIDANCE;
    bool is_laser = next_guide_type == LASER_GUIDANCE || next_guide_type == REFLECTOR_GUIDANCE;

    // 是否需要检测并获取评分
    bool needs_check = is_qr_or_mag && is_laser;

    if (!needs_check) {
        result = true;
    } else {
        // 评分低于20 需要重定位
        float score = 0.0f;
        result = GetSlamScore(score);
        if (result) {
            force_relocate = score < RELOACATE_SOCRE_LIMIT;
        } else {
            force_relocate = true;
            DEBUG_OUT("error: CheckForceRelocate GetSlamScore failed data time out; force relocate");
        }
    }

    DEBUG_OUT("CheckForceRelocate: needs_check=" << needs_check
            << ", is_qr_or_mag=" << is_qr_or_mag
            << ", is_laser=" << is_laser
            << ", current_guide_type=" << current_guide_type
            << ", next_guide_type=" << next_guide_type
            << ", getSlamScore=" << result
            << ", score=" << result);
            
    return result;
}

/**
 * @brief 获取下一个轨迹引导类型
 *
 * 根据传入的工作码指针，获取下一个轨迹的引导类型。
 *
 * @param work_code 工作码指针
 * @return 轨迹引导类型，NO_GUIDANCE 表示无引导
 */
short TaskManager::TrajGuideType(WorkCode* work_code) 
{
    if (work_code == nullptr) {
        DEBUG_WARN_OUT("NextTrajGuideType: next work code is null");
        return NO_GUIDANCE;
    }
    if (work_code->type_ != WorkCodeType::NV_WORKCODE) {
        DEBUG_WARN_OUT("NextTrajGuideType: next traj is not nv_traj");
        return NO_GUIDANCE;
    }
    auto* nv_traj = (TrajNv*)(work_code->pointer_);
    if (nv_traj == nullptr) {
        DEBUG_WARN_OUT("NextTrajGuideType: next traj nv is null");
        return NO_GUIDANCE;
    }
    short type = nv_traj->GetGuideType();
    DEBUG_OUT("NextTrajGuideType: next nv guide type(laser:0 qr:1 mag:2 noguide:7 type=" << type);
    return type;
}

/**
 * @brief 从工作缓冲区中找到第一个类型为NV_WORKCODE的WorkCode对象
 *
 * 该函数首先尝试获取工作缓冲区头部的WorkCode对象，然后检查该对象的类型是否为NV_WORKCODE。
 * 如果类型匹配，则直接返回该对象；否则，继续在工作缓冲区中查找下一个类型为NV_WORKCODE的WorkCode对象。
 * 如果找不到，则返回nullptr。
 *
 * @return 如果找到类型为NV_WORKCODE的WorkCode对象，则返回该对象的指针；否则返回nullptr。
 */
WorkCode* TaskManager::FindHeadNvWorkCode() 
{
    USHORT pos_next = work_buffer_.HeadPos();
    WorkCode* next_work_code = work_buffer_.PeekItem(pos_next);
    if (next_work_code == nullptr) {
        DEBUG_WARN_OUT("FindHeadNvWorkCode: next is null");
        return next_work_code;
    }
    if (next_work_code->type_ == WorkCodeType::NV_WORKCODE) {
        DEBUG_WARN_OUT("FindHeadNvWorkCode: find nv_code ok once");
        return next_work_code;
    }
    DEBUG_WARN_OUT("FindHeadNvWorkCode: head pos is not nv code; ready to find next...");
    next_work_code = FindNextNvWorkCode(pos_next);
    if (next_work_code == nullptr) {
        DEBUG_WARN_OUT("FindHeadNvWorkCode: cannot find next nv code;");
    }
    return next_work_code;
}


/**
 * @brief 从工作缓冲区拷贝中找到第一个类型为NV_WORKCODE的WorkCode
 * 
 * 类同FindHeadNvWorkCode
 *
 * @return 返回指向找到的 NV 类型工作代码的指针，如果未找到则返回 NULL
 */
WorkCode *TaskManager::FindHeadNvWorkCodeCopy()
{
    USHORT pos_next = work_buffer_copy_.HeadPos();
    WorkCode *next_work_code = work_buffer_copy_.PeekItem(pos_next);

    if (next_work_code == NULL) {
        DEBUG_WARN_OUT("FindHeadNvWorkCode: next is null");
        return next_work_code;
    }

    if (next_work_code->type_ ==  WorkCodeType::NV_WORKCODE) {
        DEBUG_WARN_OUT("FindHeadNvWorkCode: find nv_code ok once");
        return next_work_code;
    }

    DEBUG_WARN_OUT("FindHeadNvWorkCode: head pos is not nv code;ready to find next...");
    next_work_code = FindNextNvWorkCode(pos_next);

    if (next_work_code == NULL) {
        DEBUG_WARN_OUT("FindHeadNvWorkCode: can not find next nv code;");
    }

    return next_work_code;
}

/**
 * @brief 在工作缓冲区中查找下一个NV类型的WorkCode
 *
 * 从给定位置开始，跳过静态操作，查找下一个类型为NV_WORKCODE的WorkCode。
 *
 * @param pos_next 输入参数，指定开始查找的位置
 * @return 指向找到的下一个类型为NV_WORKCODE的WorkCode的指针，如果未找到则返回nullptr
 */
WorkCode* TaskManager::FindNextNvWorkCode(USHORT& pos_next) 
{
    // Skip static operation
    pos_next = work_buffer_.NextEntry(pos_next);
    WorkCode* next_work_code = work_buffer_.PeekItem(pos_next);
    while (next_work_code != nullptr && next_work_code->type_ != WorkCodeType::NV_WORKCODE) {
        pos_next = work_buffer_.NextEntry(pos_next);
        next_work_code = work_buffer_.PeekItem(pos_next);
    }
    return next_work_code;
}

/**
 * @brief 根据移动方向和轨迹类型设置结束速度
 *
 * 根据当前节点和下一个节点的移动方向和轨迹类型，计算并设置结束速度。
 *
 * @param cur_nv 当前节点指针
 * @param next_nv 下一个节点指针
 * @param end_vel 引用传递的结束速度
 */
void TaskManager::SetEndVelByMoveDirAndTrajType(TrajNv* cur_nv, TrajNv* next_nv, float& end_vel) 
{
    MoveDir dir1 = cur_nv->GetMoveDir();
    MoveDir dir2 = next_nv->GetMoveDir();
    short cur_nv_traj_type = cur_nv->GetTrajType();
    short next_nv_traj_type = next_nv->GetTrajType();
    if (dir1 != dir2) {  // MoveDir different
        end_vel = 0;
    } else {  // MoveDir same
        end_vel = std::min(cur_nv->GetLinearVel(), next_nv->GetLinearVel());
        end_vel = std::min(end_vel, cur_nv_->profiler_.GetEndValue());
        bool set_end_vel_zero1 = next_nv_traj_type == static_cast<short>(TrajType::SIDE_TRAJ) &&
                                    cur_nv_traj_type == static_cast<short>(TrajType::LINE_TRAJ);
        bool set_end_vel_zero2 = next_nv_traj_type == static_cast<short>(TrajType::LINE_TRAJ) &&
                                    cur_nv_traj_type == static_cast<short>(TrajType::SIDE_TRAJ);
        bool set_end_vel_zero3 = next_nv_traj_type == static_cast<short>(TrajType::SPIN_TRAJ);
        bool set_end_vel_zero = set_end_vel_zero1 || set_end_vel_zero2 || set_end_vel_zero3;
        if (set_end_vel_zero) {
            end_vel = 0;
            DEBUG_OUT("load cur_nv set end vel 0.0; cur_nv_traj_type=" << cur_nv_traj_type
                    << ", next_nv_traj_type:" << next_nv_traj_type);
        }
        DEBUG_OUT("load nv_obj end vel: " << end_vel);
    }
}

/**
 * @brief 反向角度
 *
 * 根据当前轨迹和下一个开始角度，判断是否需要对当前轨迹进行反向转向。
 *
 * @param cur_traj 当前轨迹指针
 * @param ang_next_start 下一个开始角度
 *
 * @return 如果当前轨迹需要保留转向则返回true，否则返回false
 */
bool TaskManager::SideReserveSteer(Trajectory* cur_traj, Angle ang_next_start) 
{
    if (!cur_traj || cur_traj->GetType() != static_cast<short>(TrajType::SIDE_TRAJ)) {
        return false;
    }
    SideTrajectory* cur_side_traj = dynamic_cast<SideTrajectory*>(cur_traj);
    if (!cur_side_traj) {
        DEBUG_ERROR_OUT("error: SideReserveSteer: cur_side_traj is null");
        return false;
    }
    Angle ang_cur = cur_side_traj->StartSteerAngle();
    // Reverse steer angle
    if (AngleDiff(ang_cur.radian_, ang_next_start.radian_) >
        AngleDiff((!ang_cur).radian_, ang_next_start.radian_)) {
        cur_side_traj->ReserveSteer();
        DEBUG_OUT("SideReserveSteer is ok; ang_cur.radian_=" << ang_cur.radian_
                    << ", ang_next_start.radian_=" << ang_next_start.radian_);
        return true;
    }
    return false;
}

/**
 * @brief 尝试反转侧移轨迹的转向角
 *
 * 根据当前轨迹和下一个轨迹的信息，尝试反转侧移轨迹的转向角，以实现平滑过渡。
 *
 * @param cur_nv 当前轨迹节点指针
 * @param next_nv 下一个轨迹节点指针
 * @param pos_next 下一个轨迹节点在轨迹数组中的位置
 */
void TaskManager::TryToReverseSideSteerAngle(TrajNv* cur_nv, TrajNv* next_nv, USHORT& pos_next) 
{
    if (!cur_nv || cur_nv->GetTrajType() != static_cast<short>(TrajType::SIDE_TRAJ) || side_sooth_.use_last_end_steer_ang) {
        return;
    }

    SideTrajectory* cur_side_traj = dynamic_cast<SideTrajectory*>(cur_nv->GetTraj());
    if (!cur_side_traj) {
        DEBUG_ERROR_OUT("Error: TryToReverseSideSteerAngle: cur_side_traj is null");
        return;
    }

    if (!cur_side_traj->CanSteerReserve()) {
        DEBUG_OUT("TryToReverseSideSteerAngle: cur_side_traj cannot SteerReserve");
        return;
    }

    Trajectory* trajs[WORK_BUFFER_NUM];  // Max buffer is 50
    int index = 0;
    while (next_nv != nullptr) {
        trajs[index] = next_nv->GetTraj();
        if (!SideCanSteerReserve(trajs[index])) {
            DEBUG_OUT("TryToReverseSideSteerAngle: index=" << index << ", cannot reserve or other traj, exit while");
            break;
        }

        WorkCode* next_work_code = FindNextNvWorkCode(pos_next);
        if (next_work_code == nullptr) {
            next_nv = nullptr;  // End: exit while(next_nv != nullptr)
            DEBUG_OUT("TryToReverseSideSteerAngle: index=" << index << ", next_work_code is null; exit while");
            break;  // Add by yxj
        } else {
            next_nv = (TrajNv*)(next_work_code->pointer_);
            ++index;
        }
    }

    // #2
    // side->side->side->side->curSideTraj
    // 若后续路径以SIDE_TRAJ结束，且舵角可以翻转，则启用对上段路径结束舵角的使用
    //
    bool can = SideCanSteerReserve(trajs[index]);
    if (can) {
        side_sooth_.use_last_end_steer_ang = true;
        side_sooth_.last_end_steer_ang = cur_side_traj->StartSteerAngle();
    } else {
        // 如果最后一个路径为空 或不是侧移 或 是侧移不能反转
        // 从后向前对SIDE_TRAJ舵角进行规划，以实现平稳过渡
        while (index >= 0) {
            Angle ang_next_start = trajs[index]->StartSteerAngle();
            bool is_ok = index > 0 ? SideReserveSteer(trajs[index - 1], ang_next_start) : SideReserveSteer(cur_side_traj, ang_next_start);
            --index;
            DEBUG_OUT("index:" << index << " reverse:" << is_ok);
        }
    }
}

/**
 * @brief 使用上一次结束的转向角处理侧移轨迹
 *
 * 根据当前轨迹的类型和状态，决定是否需要翻转侧移舵角，并更新上一次结束的转向角。
 *
 * @param cur_nv 当前轨迹节点指针
 */
void TaskManager::SideUseLastEndSteerAng(TrajNv* cur_nv) 
{
    if (cur_nv == nullptr) {
        DEBUG_ERROR_OUT("SideUseLastEndSteerAng: cur_nv is null");
        return;
    }

    short cur_traj_type = cur_nv->GetTrajType();
    switch (cur_traj_type) {
        case static_cast<short>(TrajType::SIDE_TRAJ): {
            if (!side_sooth_.use_last_end_steer_ang) {
                DEBUG_OUT("side traj: use_last_end_steer_ang is false");
                return;
            }

            SideTrajectory* cur_side_traj = dynamic_cast<SideTrajectory*>(cur_nv->GetTraj());
            if (cur_side_traj == nullptr) {
                DEBUG_ERROR_OUT("error: SideUseLastEndSteerAng: cur_side_traj is null");
                return;
            }

            // 试图翻转侧移舵角
            Angle cur_angle = cur_side_traj->StartSteerAngle();
            if (cur_side_traj->CanSteerReserve()) {
                if (AngleDiff(side_sooth_.last_end_steer_ang.radian_, cur_angle.radian_) >
                    AngleDiff(side_sooth_.last_end_steer_ang.radian_, (!cur_angle).radian_)) {
                    DEBUG_OUT("ready reserve steer: last_angle=" << side_sooth_.last_end_steer_ang.radian_
                            << ", cur_angle=" << cur_angle.radian_);
                    cur_side_traj->ReserveSteer();
                }
            }

            // Update angle; if can reserve, it is reserved angle
            side_sooth_.last_end_steer_ang = cur_side_traj->StartSteerAngle();
            DEBUG_OUT("side traj: last steer angle (degree)=" << side_sooth_.last_end_steer_ang.Degree());
        } break;

        case static_cast<short>(TrajType::BEZIER_TRAJ): {
            BezierTrajectory* cur_bezier_traj = dynamic_cast<BezierTrajectory*>(cur_nv->GetTraj());
            if (cur_bezier_traj != nullptr && cur_bezier_traj->IsShiftMove()) {
                // The marker can use the end rudder angle value to plan the next path
                side_sooth_.use_last_end_steer_ang = true;
                side_sooth_.last_end_steer_ang = cur_bezier_traj->EndSteerAngle();
                DEBUG_OUT("SideUseLastEndSteerAng: bezier shift move, set end angle (degree)="
                            << side_sooth_.last_end_steer_ang.Degree());
            }
        } break;

        default: {
            side_sooth_.use_last_end_steer_ang = false;
            DEBUG_OUT("SideUseLastEndSteerAng: other traj: set use_last_angle false; cur_traj_type="
                    << cur_traj_type);
        } break;
    }
}

//////////////////////////////////////////////////////////////////////////////
//   Part 3. Work code execution.
/**
 * @brief TimerTask 函数用于管理任务管理器中的定时器任务
 *
 * 该函数主要负责处理定时器任务，包括编译任务顺序、修改末端速度、检查任务连接、全局速度规划、
 * 加载新任务、执行任务前的准备、执行当前工作代码、更新工作状态、处理其他任务以及处理控制台发出的删除任务指令。
 *
 * @note 该函数记录任务开始和结束的时间，并计算任务执行所需的时间。
 */
void TaskManager::TimerTask()
{
    double starttime = ROSTime().seconds();
    DEBUG_OUT("TimerTask startTime(s):" << starttime);
    // #2 compile max 8 tasks (20-12)
    CompileOrderTaskProc();

    // #3 modify end vel
    if (work_buffer_copy_.GetCount() > 0) {
        ComputeLastNvEndVel();
    }

    // 检测是否由于任务拼接，需要重新规划目标0速度
    CheckTaskConnect();

    // 全局速度规划
    RunCurvePlan();

    // 检测末端停车距离不够是否需要重规划问题
    // CheckMinDisDecReplan();

    // #4 load a new one task
    LoadNewOrderTaskProc();

    // #4.1 task ready to run to do something
    PreDoingProc();

    // #5 In execution state
    RunCurWorkCodeProc();

    // #4 update work state
    UpdateWorkState();

    OtherDoingProc();

    // #5 Handle the delete task instruction issued by the console
    HandleEndTask();

    double endtime = ROSTime().seconds();
    DEBUG_OUT("TimerTask endTime(s):" << endtime << ",diffTime=" << endtime - starttime);
}

/**
 * @brief 编译任务处理函数
 *
 * 此函数负责处理编译任务队列中的任务。
 *
 * 如果当前编译任务被暂停，则输出调试信息并返回。
 * 循环处理编译任务，直到满足以下条件之一：
 * 1. 当前编译任务被暂停。
 * 2. 编译任务缓冲区为空。
 * 3. 工作缓冲区中的任务数量超过指定阈值（WORK_BUFFER_NUM - 40）。
 *
 * 在每次循环中，调用 OnlineCompileOrderTask() 函数尝试在线编译任务。
 * 如果编译失败，则清空编译任务缓冲区和任务ID缓冲区，并清除最新的订单信息。
 * 同时输出错误信息，并调用 IdleTryToOccurError() 函数处理错误情况。
 * 如果编译成功，则清空错误详情，并调用 IdleTryToOccurError() 函数表示没有错误发生。
 */
void TaskManager::CompileOrderTaskProc()
{
    // # compile max 8 tasks (20-12)
    if (pause_cur_compile_) {
        DEBUG_OUT("CompileOrderTaskProc: pause compile current task");
        return;
    }

    while (!pause_cur_compile_ && !order_task_buffer_.IsEmpty() && 
            (work_buffer_.GetCount() <= WORK_BUFFER_NUM - 40)) {
        if (!OnlineCompileOrderTask()) {
            order_task_buffer_.RemoveAll();
            task_id_buffer_.RemoveAll();
            VhcStatus.rcs_fb_.Clear();  // Clear the latest order information
            ForceClearCurTaskId();
            DEBUG_ERROR_OUT("online=" << online_ 
                            << ", remove all later order task code; Compile failed order task code is error; taskErrDetail" 
                            << task_err_detail_);
            IdleTryToOccurError(true, task_err_detail_);
        } else {
            task_err_detail_.clear();
            IdleTryToOccurError(false, task_err_detail_);
        }
    }
}

/**
 * @brief 加载新订单任务处理函数
 *
 * 根据当前工作状态和工作缓冲区的情况，决定是否需要加载新的工作代码。
 *
 * 如果当前没有正在执行的工作代码，工作缓冲区不为空，并且没有因为动态操作而暂停，
 * 则根据工作缓冲区头部的工作代码类型来决定是否加载新的工作代码。
 *
 * 支持的工作代码类型包括导航代码、操作代码和动态操作代码。
 *
 * 如果遇到相邻的两个动态操作代码，则丢弃第二个动态操作代码，因为任务重复。
 *
 * 如果遇到不符合预期的情况，则输出错误信息。
 */
void TaskManager::LoadNewOrderTaskProc()
{
    bool cur_work_run = cur_work_code_.IsExecutable();
    bool work_buffer_empty = work_buffer_.IsEmpty();

    if (!cur_work_run && !work_buffer_empty && !suspend_for_dyn_op_) {
        WorkCode* peek_head_work_code = work_buffer_.PeekItem(work_buffer_.HeadPos());
        WorkCodeType work_code_type = peek_head_work_code ? peek_head_work_code->type_ : WorkCodeType::NO_WORKCODE;
        bool is_nav_code = work_code_type == WorkCodeType::NV_WORKCODE;
        bool is_op_code = work_code_type == WorkCodeType::OP_WORKCODE && cur_dyn_op_ == nullptr;
        bool is_dyn_op_code = work_code_type == WorkCodeType::DYN_WORKCODE && cur_dyn_op_ == nullptr && cur_op_ == nullptr;

        if (is_nav_code || is_op_code || is_dyn_op_code) {
            // #1 Not in execution state
            cur_work_code_ = work_buffer_.Get();  // FIXME: dyn op task update cur task id maybe problem
            if (cur_work_code_.IsStartOfTask()) {
                DEBUG_OUT("#1 StartOfTask: ready to update current task id");
                UpdateCurTaskId();
            }
            DEBUG_OUT("Ready to load work object: type (0-None; 1-Navigation code; 2-Operation code; 3-Dynamic code)="
                        << static_cast<int>(cur_work_code_.type_) << " taskFlag (0 - normal; 1 - Start of task; 2 - end of task):" 
                        << static_cast<int>(cur_work_code_.task_flag_));
            switch (cur_work_code_.type_) {
                case WorkCodeType::NV_WORKCODE:
                    LoadNvObj((TrajNv*)(cur_work_code_.pointer_));
                    break;
                case WorkCodeType::OP_WORKCODE:
                    LoadOpObj((Operation*)(cur_work_code_.pointer_));
                    break;
                case WorkCodeType::DYN_WORKCODE:
                    LoadDynOpObj((Operation*)(cur_work_code_.pointer_));
                    break;
                default:
                    break;
            }
        } else if (work_code_type == WorkCodeType::DYN_WORKCODE && cur_dyn_op_) {
            // 如果发生相邻的两个动态动作
            // 则扔掉第二个动态动作，因为任务重复
            auto next_dyn_code = work_buffer_.Get();  // FIXME: dyn op task update cur task id maybe problem
            if (next_dyn_code.IsStartOfTask()) {
                DEBUG_OUT("#2 nextDynCode StartOfTask: ready to update current task id");
                UpdateCurTaskId();
            }
            DEBUG_ERROR_OUT("Error: LoadNewOrderTaskProc; current is dyn op; nextWorkCode is dyn op; same dyn op;");
        } else {
            DEBUG_ERROR_OUT("Error: logical error; LoadNewOrderTaskProc");
        }
    }
}

/**
 * @brief 运行当前工作码
 *
 * 该函数负责根据当前工作码的类型执行。
 *
 * @note
 * - 如果当前工作代码是可执行的，则根据其类型执行相应的处理函数。
 * - 如果当前工作代码为空，则尝试加载一个新的非易失性任务。
 * - 注意：RunCurNvObj 和 RunCurDynOpObj 可能同时运行，需要特别注意并发问题。
 */
void TaskManager::RunCurWorkCodeProc()
{
    // #2 In execution state
    if (cur_work_code_.IsExecutable()) { 
        switch (cur_work_code_.type_) {
            case WorkCodeType::NV_WORKCODE:
                RunCurNvObj(); // finish -> Nullify: Executable to Unenforceable
                break;
            case WorkCodeType::OP_WORKCODE:
                RunCurOpObj();
                break;
            default:
                break;
        }
    }

    // #3 when dyn op m_CurWorkCode is null ; it can to load a new nv task
    //  FIXME: RunCurNvObj and RunCurDynOpObj can be running in the meantime
    RunCurDynOpObj();
}

/**
 * @brief 加载轨迹导航对象
 *
 * 该函数用于加载轨迹导航对象，并进行一系列初始化操作。
 *
 * @param nv 指向TrajNv对象的指针，表示要加载的轨迹导航对象。
 */
void TaskManager::LoadNvObj(TrajNv* nv)
{
    std::lock_guard<std::mutex> locker(run_obj_mutex_);

    cur_nv_ = nv;
    if (!cur_nv_) {
        DEBUG_OUT("LoadNvObj is nullptr");
        return;
    }

    // #1 temp diff do nothing: for diff drive is useless
    SideUseLastEndSteerAng(cur_nv_);

    // #2 设置是否开启随动
    UpdateRackFollowUp(cur_nv_);

    // #3  try to modify the node 0 endVel of the current section
    //    reverse side steer angle
    short cur_traj_type = cur_nv_->GetTrajType();
    USHORT next_pos = work_buffer_.HeadPos();
    WorkCode* next_work_code = work_buffer_.PeekItem(next_pos);
    float end_vel = 0.0f;

    // FIXME: add by qf filter dyn op
    while (next_work_code) {
        if (next_work_code->type_ == WorkCodeType::DYN_WORKCODE) {
            next_pos = work_buffer_.NextEntry(next_pos);
            next_work_code = work_buffer_.PeekItem(next_pos);
        } else {
            break;
        }
    }

    if (next_work_code) {
        bool is_nv_workcode = next_work_code->type_ == WorkCodeType::NV_WORKCODE;
        if (!is_nv_workcode) {
            end_vel = 0; // Next work code is not a nv workcode
        } else {
            TrajNv* next_nv = (TrajNv*)(next_work_code->pointer_);
            if (next_nv != NULL) {
                // important
                SetEndVelByMoveDirAndTrajType(cur_nv_, next_nv, end_vel); // SHORT nextNvTrajType = pNextNv->GetTrajType(); SetEndVelByTrajType(curNvtrajType,nextNvTrajType,fEndVel);
                TryToReverseSideSteerAngle(cur_nv_, next_nv, next_pos);
            }
        }
    }

    // #3 In spin mode,set up the end velocity equal to zero
    if (cur_traj_type == (short)TrajType::SPIN_TRAJ) {
        end_vel = 0;
        // 是否根据实际起始角度 算出自旋的方向
        TryModifySpinDir(cur_nv_->GetTraj());
    }
    // #1 准备检测是否重定位
    relocate_.Ready();
    // #2 清除重定位，停车标识
    SetStopCategory(StopType::STOP_RELOCATE, false);

    // The current track starts directly
    cur_nv_->LoadPlanner(&global_plan_);
    cur_nv_->Start((float)fabs((double)record_last_nv_vel_.linear_), end_vel, profile_mode_, curvature_, curvature_known_, last_traj_type_, last_steer_angle_);
    work_state_ = AGV_IS_NAVIGATE;
}

/**
 * @brief 加载操作对象
 *
 * 加载一个操作对象到任务管理器中。
 *
 * @param op 操作对象指针
 */
void TaskManager::LoadOpObj(Operation* op) {
    std::lock_guard<std::mutex> locker(run_obj_mutex_);

    if (!op || cur_op_) {
        DEBUG_OUT("Error: load static op is null or current op is running: cur_op=" 
                    << reinterpret_cast<uintptr_t>(cur_op_));
        return;
    }

    UpdateActionListStates(SetActionState(op->GetCurActionObj(), STATUS_WAIT));
    cur_op_ = op;
    work_state_ = AGV_IS_IN_OP;
    DEBUG_OUT("Start load op start action");
}

/**
 * @brief 动态加载操作对象
 *
 * 动态加载操作对象到任务管理器中，并更新当前操作状态。
 *
 * @param dyn_op 要加载的动态操作对象指针
 */
void TaskManager::LoadDynOpObj(Operation* dyn_op) {
    std::lock_guard<std::mutex> locker(run_obj_mutex_);
    
    if (!dyn_op || cur_dyn_op_) {
        DEBUG_OUT("Error: Invalid dyn_op or existing operation running");
        return;
    }

    UpdateActionListStates(SetActionState(dyn_op->GetCurActionObj(), STATUS_WAIT));
    cur_dyn_op_ = dyn_op;
    cur_work_code_.Nullify();
    
    DEBUG_OUT("Dynamic operation loaded successfully");
}

/**
 * @brief 运行当前导航对象
 *
 * 此函数负责运行当前导航对象，包括处理导航过程中的各种逻辑，如动态操作、路径规划等。
 */
void TaskManager::RunCurNvObj() 
{
    std::lock_guard<std::mutex> locker(run_obj_mutex_);
    if (!cur_nv_) return;

    // 如果当前是二维码导航，下一个阶段判断是否时激光导航，是就强制停车，等待完成。
    RelocateCycle();

    // 设定全局曲线规划速度值。
    // float curve_vel = global_plan_.PlanVel();
    // cur_nv_->SetPlanValue(curve_vel, relQ);

    // 运行当前轨迹周期。
    cur_nv_->CycleRoutine();  // check mark

    // 如果当前动态操作正在运行，尝试停止。
    TryToStopForDynOp();
    
    if (cur_nv_->Finished()) {
        // 更新下一个导航的信息。
        float end_value = cur_nv_->GetEndValue();
        record_last_nv_vel_.linear_ = cur_nv_->GetLinearVel();
        record_last_nv_vel_.angular_ = cur_nv_->GetAngularVel();
        profile_mode_ = cur_nv_->GetWorkMode();
        curvature_ = cur_nv_->EndCurvature();
        last_traj_type_ = cur_nv_->GetTrajType();
        curvature_known_ = true;
        last_steer_angle_ = cur_nv_->GetSteerAngle();

        // 添加记录结束时，起点，终点节点, 当前路段理论长度。
        USHORT start = cur_nv_->start_node_id_;
        USHORT end = cur_nv_->end_node_id_;
        float range = cur_nv_->GetTraj()->GetRange();

        ClearPidCtrl(end_value);
        UpdateRackFollowUp(nullptr);

        if (cur_work_code_.IsEndOfTask()) {
            work_state_ = AGV_IS_IDLE;
            DEBUG_OUT("cur code end task");
        }

        SuspendNvTaskForDynOp();
        DelNvTask();

        // 每当一个路段完成，清除当前工作代码，并从缓冲区中提取一个数据（用于计算节点速度）。
        cur_work_code_.Nullify();

        // 当当前导航完成时，弹出一个工作代码。
        if (work_buffer_copy_.GetCount() > 0) {
            work_buffer_copy_.Get();
        }
        global_plan_.AddNewCoverQ(range);
        
        
        // 当前规划已经结束，设置允许新的规划准备就绪。
        if (global_plan_.target_info_.PlanFinished(last_traj_type_, start, end)) {
            DEBUG_OUT("RunCurNvObj finish; agv stop, set plan ready");
            global_plan_.target_info_.SetPlanFlag(true);
        }
    }
}

/**
 * @brief 运行当前操作对象
 *
 * 此函数负责运行当前操作对象（cur_op_），并执行相关的状态更新和操作。
 *
 * @note 需要在运行前加锁，以确保线程安全。
 */
void TaskManager::RunCurOpObj()
{
    std::lock_guard<std::mutex> locker(run_obj_mutex_);

    if (!cur_op_) {
        DEBUG_OUT("can not run cur op obj;curOp is null");
        return;
    }

    if (cur_dyn_op_) {
        DEBUG_OUT("error: when static op ready run, dynatic op is not finished");
        return;
    }

    cur_op_->CycleRoutine();

    if (cur_op_->Finished()) {

        UpdateActionListStates(SetActionState(cur_op_->GetCurActionObj(), STATUS_FINISH));
        DelOpTask();

        if (cur_work_code_.IsEndOfTask()) {
            work_state_ = AGV_IS_IDLE;
        }

        cur_work_code_.Nullify();
        // 原地动作任务完成时，设置规划准备
        global_plan_.target_info_.SetPlanFlag(true);
        DEBUG_OUT("cur static op is finished; set plan ready");
    } else {
        VhcStatus.task_status_ = TaskStatus::Run;
        UpdateActionListStates(SetActionState(cur_op_->GetCurActionObj(), STATUS_RUN));
    }
}

/**
 * @brief 运行当前动态操作对象
 *
 * 该函数负责运行当前动态操作对象。如果当前存在动态操作对象，则执行其循环例程。
 * 如果动态操作对象执行完毕，则更新操作列表状态，释放动态操作对象，并处理相关资源。
 *
 * @note 该函数需要确保线程安全，因此在执行过程中使用了互斥锁。
 */
void TaskManager::RunCurDynOpObj()
{
    std::lock_guard<std::mutex> locker(run_obj_mutex_);

    if (!cur_dyn_op_) return;

    if (cur_op_) {
        DEBUG_ERROR_OUT("error: when dyn op ready run, static op is not finished");
        return;
    }

    cur_dyn_op_->CycleRoutine();

    if (cur_dyn_op_->Finished()) {
        UpdateActionListStates(SetActionState(cur_dyn_op_->GetCurActionObj(), STATUS_FINISH));
        suspend_for_dyn_op_ = FALSE;
        DelDynTask();

        if (cur_nv_ && cur_nv_->OverrideEndVel())
        {
            cur_nv_->SetOverrideEndVel(false);
            DEBUG_OUT("cur dynamic op finish  to realease override end vel flag ");
        }

        DEBUG_OUT("cur dynamic op is finished ");
    }
    else
    {
        UpdateActionListStates(SetActionState(cur_dyn_op_->GetCurActionObj(), STATUS_RUN));
    }
}

/**
 * @brief 暂停导航任务以进行动态操作
 *
 * 该函数用于在导航过程中暂停当前任务，以便执行动态操作。
 *
 * @note 当存在当前动态操作且尚未完成时，如果当前动态操作到达结束节点，
 *       则暂停导航任务，否则继续执行导航任务。
 */
void TaskManager::SuspendNvTaskForDynOp() {
    if (!cur_dyn_op_) {
        return;
    }

    bool arrive_end_node = cur_dyn_op_->GetDynOpEndNode() == VhcStatus.to_node_;
    bool dyn_op_running = !cur_dyn_op_->Finished();

    if (arrive_end_node && dyn_op_running) {
        suspend_for_dyn_op_ = true;
        DEBUG_OUT("Dynamic operation forced navigation task suspension");
    } else {
        suspend_for_dyn_op_ = false;
    }
}

/**
 * @brief 尝试修改轨迹的自旋方向
 *
 * 该函数尝试修改传入的轨迹对象的自旋方向。如果轨迹不是自旋轨迹或者轨迹为空，函数将直接返回。
 *
 * @param cur_traj 指向要修改的轨迹对象的指针
 */
void TaskManager::TryModifySpinDir(Trajectory* cur_traj) 
{
    if (!cur_traj) {
        DEBUG_ERROR_OUT("TryModifySpinDir error: traj is null");
        return;
    }

    if (cur_traj->GetType() != static_cast<short>(TrajType::SPIN_TRAJ)) {
        DEBUG_ERROR_OUT("TryModifySpinDir error: traj is not spin traj"); 
        return;
    }

    try {
        DEBUG_OUT("TryModifySpinDir: get pose start");
        float x = 0.0f;
        float y = 0.0f; 
        float theta = 0.0f;
        float quality = 0.0f;

        // 获取激光定位姿态,quality>5.0才认为有效
        bool valid_pose = GetLaserAutoPos(x, y, theta, quality, false, true) && 
                            quality > 5.0f;
        if (valid_pose) {
            auto* spin_traj = dynamic_cast<SpinTrajectory*>(cur_traj);
            bool modified = spin_traj->ModifySpinDir(theta);
            DEBUG_ERROR_OUT("TryModifySpinDir ok; modified=" << modified);
        } else {
            DEBUG_ERROR_OUT("TryModifySpinDir error: GetLaserAutoPos failed");
        }
    } catch (...) {
        DEBUG_ERROR_OUT("TryModifySpinDir error: crash"); 
    }
}
//
// During dynamic subpackage or dynamic process,
// the remaining road distance is detected in real time before reaching the dynamic end point,
// If it is estimated that it cannot be completed normally, stop at the end point
//
void TaskManager::TryToStopForDynOp()
{
    if (!cur_dyn_op_) return; 
    if (!cur_nv_) {
        DEBUG_OUT("error:TryToStopForDynOp: cur Nv is null");
        return;
    }

    // #1 if it is to finish node
    //  Real time calculation of the distance between the end point of the road section and the end point of the dynamic task
    //  Modify the end speed of the current section when it is less than 1.4m
    USHORT to_node = VhcStatus.to_node_;
    USHORT finish_node = cur_dyn_op_->GetDynOpEndNode();
    bool ready_stop = finish_node == to_node;
    float dis_to_node = 1.45, modify_value = 0.4f;
    if (ready_stop) {
        cur_nv_->SetOverrideEndVel(TRUE);
        cur_nv_->ModifyEndValue(0.0); // use
        DEBUG_OUT("TryToStopForDynOp: modify end vel 0.0 ;cur vel:" << cur_nv_->GetLinearVel());
        return;
    }

    // #2 it is not to finish node
    Node* to_node_pointer = world.GetNode(to_node);
    Node* finish_node_pointer = world.GetNode(finish_node);

    if (!to_node_pointer) {
        DEBUG_ERROR_OUT("error: TryToStopForDynOp failed; pToNode is null;uToNode=" << to_node);
        return;
    }

    if (!finish_node_pointer) {
        DEBUG_ERROR_OUT("error: TryToStopForDynOp failed; pFinishNode is null;uFinishNode=" << finish_node);
        return;
    }

    Node nod_to = *to_node_pointer;
    Node nod_finish = *finish_node_pointer;
    float dis_x = fabsf(nod_to.x_ - nod_finish.x_);
    float dis_y = fabsf(nod_to.y_ - nod_finish.y_);
    bool is_use_x = dis_x > dis_y;
    // FIXME: change by yxj
    float max_dis = dis_x > dis_y ? dis_x : dis_y;
    if (max_dis < dis_to_node && cur_nv_->GetEndValue() > modify_value) {
        float old_end_value = cur_nv_->GetEndValue();
        cur_nv_->SetOverrideEndVel(true);
        cur_nv_->ModifyEndValue(modify_value); // use
        DEBUG_OUT(" dyn op: bUse_x=" << is_use_x << ",modify 0.4; cur vel:" << cur_nv_->GetLinearVel()
                                     << ",dis_x=" << dis_x << ",dis_y=" << dis_y << ",disToNode(threshold)=" << dis_to_node << ",modifyValue=" << modify_value
                                     << ",fOldEndValue=" << old_end_value);
    }
}

/**
 * @brief 更新车辆工作状态
 *
 * 更新车辆的工作状态，包括当前工作状态和是否空闲的状态。
 */
void TaskManager::UpdateWorkState()
{
    VhcStatus.work_state_ = work_state_;
    VhcStatus.is_idle_ = IsIdle();
}

//////////////////////////////////////////////////////////////////////////////

/**
 * @brief 判断编译后的轨迹是否与当前轨迹或尾部轨迹的转向角度匹配
 *
 * 判断编译后的轨迹（compile_nv）的起始转向角度与当前轨迹或尾部轨迹的结束转向角度是否平滑过渡，
 * 即两者的差值是否在最大平滑转向角度阈值（MAX_STEER_ANGLE_SMOOTH）之内。
 *
 * @param compile_nv 编译后的轨迹指针
 * @return 如果匹配返回true，否则返回false
 */
bool TaskManager::SteerAngleFit(TrajNv* compile_nv) {
    if (!HasSteer()) {
        return true; 
    }

    if (!compile_nv) {
        DEBUG_ERROR_OUT("Error: SteerAngleFit compile_nv is null");
        return false;
    }

    WorkCode* tail_work_code = FindTailWorkCode();
    // Compare with current navigation
    if (!tail_work_code) {
        if (!cur_nv_) {
            DEBUG_ERROR_OUT("Error: SteerAngleFit cur_nv_ is null"); 
            return false;
        }
        
        Angle end_angle = cur_nv_->GetTraj()->EndSteerAngle();
        Angle start_angle = compile_nv->GetTraj()->StartSteerAngle();
        float angle_diff = fabs(NormAngle2(end_angle.radian_ - start_angle.radian_));
        angle_diff = TO_DEGREE(angle_diff);
        return angle_diff < MAX_STEER_ANGLE_SMOOTH;
    }

    // Compare with tail navigation
    TrajNv* tail_nv = (TrajNv*)(tail_work_code->pointer_);
    Angle end_angle = tail_nv->GetTraj()->EndSteerAngle();
    Angle start_angle = compile_nv->GetTraj()->StartSteerAngle();
    float angle_diff = fabs(NormAngle2(end_angle.radian_ - start_angle.radian_));
    angle_diff = TO_DEGREE(angle_diff);
    return angle_diff < MAX_STEER_ANGLE_SMOOTH;
}

/**
 * @brief 检查轨迹是否满足航向拟合条件
 *
 * 检查给定的轨迹是否满足航向拟合条件。
 *
 * @param compile_nv 指向TrajNv对象的指针，包含待检查的轨迹信息
 * @return 如果轨迹满足航向拟合条件，则返回true；否则返回false
 *
 * 检查给定的轨迹对象是否非空，然后计算当前编译的轨迹起始航向与上一次编译的航向之间的角度差。
 * 如果角度差小于等于最大平滑航向角度（MAX_HEAD_ANGLE_SMOOTH，以度为单位），则认为满足航向拟合条件，返回true；
 * 否则返回false。
 *
 * 注意：此函数目前不处理特定类型的轨迹（如BEZIER_TRAJ）和动态位置类型（DYNAMIC_LOCATION）的组合情况。
 */
bool TaskManager::HeadingFit(TrajNv* compile_nv)
{
    if (!compile_nv) {
        DEBUG_ERROR_OUT("Heading cannot fit: compile_nv is null");
        return false;
    }

    //    if (pCompileNv->GetTraj()->GetType() == BEZIER_TRAJ && dyn_virt_status_.m_nLocType == DYNAMIC_LOCATION)
    //    {
    //        DEBUG_OUT("dyn location : BEZIER_TRAJ: heading can fit");
    //        return TRUE;
    //    }
    // The angDiff between the currently compiled and dyn_virt_status_(last compiled) objects
    const Angle heading = compile_nv->GetTraj()->StartHeading();
    const Angle angle_diff = abs(heading - dyn_virt_status_.cur_posture_.GetAngle());
    return (angle_diff <= Angle(MAX_HEAD_ANGLE_SMOOTH, AngleMode::InDegree));
}

/**
 * @brief 判断新的编译导航指令（compile_nv）与当前工作缓冲区尾部的导航指令（nv_obj）的方向是否匹配
 *
 * 从工作缓冲区的尾部开始查找 nv_obj。
 *
 * @param compile_nv 指向新的编译导航指令的指针
 *
 * @return 如果方向匹配，则返回 true；否则返回 false
 */
bool TaskManager::WheelMoveDirFit(TrajNv* compile_nv)
{
    // from work buffer tail to find nv_obj
    WorkCode* tail_work_code = FindTailWorkCode();

    // #1 when work buffer is no nvObj; new compile nvObj is nextNvObj
    if (!tail_work_code) {
        // if fisrt find tail work code ;0-buffersize = 19
        // pTailWorkCode == 0
        if (!cur_nv_) {
            DEBUG_OUT("wheel move dir not match or begin to task work code is null");
            return false;
        } else {
            bool dir_fit = cur_nv_->GetTraj()->WheelMoveDir() == compile_nv->GetTraj()->WheelMoveDir();
            return dir_fit;
        }
    } else {
        // #2 when work buffer tail pos nvObj; new compile nvObj is nextNvObj
        TrajNv *tail_nv = (TrajNv *)(tail_work_code->pointer_);
        bool dir_fit = tail_nv->GetTraj()->WheelMoveDir() == compile_nv->GetTraj()->WheelMoveDir();
        return dir_fit;
    }

    return FALSE;
}

/**
 * @brief 在任务管理器中查找最后一个非NV类型的工作码
 *
 * 在任务管理器的工作缓冲区中，查找最后一个非NV类型的工作码。
 *
 * @return 指向最后一个非NV类型工作码的指针，如果未找到则返回nullptr。
 */
WorkCode* TaskManager::FindTailWorkCode()
{
    uint16_t tail_pos = work_buffer_.TailPos();
    uint16_t position = work_buffer_.LastEntry(tail_pos);
    WorkCode* tail_work_code = work_buffer_.PeekItem(position);

    // 当最后一个时动态码，且无后续工作码
    // pTailWorkCode 为工作码
    //  (pTailWorkCode->uchType == DYN_WORKCODE || pTailWorkCode->uchType == OP_WORKCODE || pTailWorkCode->uchType == NO_WORKCODE)
    while (tail_work_code != nullptr && 
            tail_work_code->type_ != WorkCodeType::NV_WORKCODE) {
        position = work_buffer_.LastEntry(position);
        tail_work_code = work_buffer_.PeekItem(position);
    }

    return tail_work_code;
}

/**
 * @brief 判断任务管理器是否处于空闲状态
 *
 * 判断任务管理器是否处于空闲状态，即没有待处理的任务，当前工作代码不可执行，
 * 当前动态操作为空，并且（在线模式下）没有待处理的任务或（离线模式下）本地订单未回收。
 *
 * @return 若任务管理器处于空闲状态，则返回 true；否则返回 false。
 */
bool TaskManager::IsIdle() 
{
  bool is_idle = order_task_buffer_.IsEmpty() && work_buffer_.IsEmpty() &&
                    !cur_work_code_.IsExecutable() && cur_dyn_op_ == nullptr;

  is_idle = online_ ? is_idle : (!local_order_.Recycle() && is_idle);

  return is_idle;
}

/**
 * @brief 检查任务是否有效
 *
 * 检查给定的任务ID和子任务ID是否有效。
 *
 * @param task_id 任务ID
 * @param sub_task_id 子任务ID
 * @return 如果任务有效，则返回true；否则返回false
 */
bool TaskManager::TaskValid(long long task_id, USHORT sub_task_id)
{
    bool is_valid = true;
    bool is_empty = task_id_buffer_.IsEmpty();
    
    if (is_empty) {
        bool is_same = cur_task_id_ == task_id && 
                        cur_sub_task_id_ == sub_task_id;
        is_valid = !is_same;
        return is_valid;
    }

    uint16_t head_pos = task_id_buffer_.HeadPos();
    uint16_t tail_pos = task_id_buffer_.TailPos();
    uint16_t current_pos = tail_pos;

    // from tail pos to head pos to traversal query
    // #1 firet judge is task is the same as taskIdBuf
    while (current_pos != head_pos) {
        TaskId* prev_data = task_id_buffer_.PeekItem(
            task_id_buffer_.LastEntry(current_pos));
            
        bool is_same = prev_data->task_id == task_id && 
                    prev_data->subtask_id == sub_task_id;
        if (is_same) {
            is_valid = false;
            DEBUG_OUT("Task duplicate: id=" << task_id << 
                        ", sub_id=" << sub_task_id);
            break;
        }
        // if The tail is in front of the head
        USHORT pos = (current_pos == 0) ? (task_id_buffer_.GetSize() - 1) : (--current_pos);
        current_pos = pos;
    }

    return is_valid;
}

/**
 * @brief 处理离线任务发送请求
 *
 * 该函数接收一个agv_srvs::srv::LocalOrder::Request类型的请求，并根据请求内容处理离线任务的发送。
 *
 * @param req agv_srvs::srv::LocalOrder::Request类型的请求，包含离线任务的相关信息。
 *
 * @return 如果任务发送成功，则返回true；否则返回false。
 */
bool TaskManager::OnSendOfflineTask(const agv_srvs::srv::LocalOrder::Request &req)
{
    DEBUG_OUT("OnSendOfflineTask start:use online order logic");
    local_order_.recycle = req.recycle;
    local_order_.order = req.order;
    bool ret = OnSendOnlineOrderTask(local_order_.order);
    DEBUG_WARN_OUT("OnSendOfflineTask end;ret=" << ret << ",recycle=" << local_order_.recycle);
    DEBUG_WARN_OUT("offline task:" << local_order_.order);
    return ret;
}

/**
 * @brief 处理发送在线订单任务的函数
 *
 * 该函数处理发送的在线订单任务，并更新相关状态和信息。
 *
 * @param order_task 订单任务消息，包含订单ID和子任务ID等信息
 *
 * @return 处理成功返回true，否则返回false
 */
bool TaskManager::OnSendOnlineOrderTask(agv_msgs::msg::Order order_task)
{
    std::lock_guard<std::mutex> locker(order_mutex_);

    int sub_task_id; 
    long long task_id;
    if (!string2Number(order_task.orderid, task_id)) {
        DEBUG_OUT("error: orderid is not number,orderid:" << order_task.orderid);
        return false;
    }

    sub_task_id = order_task.orderupdateid;
    if (!TaskValid(task_id, sub_task_id)) {
        DEBUG_OUT("error: run the same order task;task can not add order buffer;");
        LogOutCurOrder(order_task);
        return false;
    }

    DEBUG_OUT("ready add new order task : task id valid,nTaskID:"
              << task_id << ",nSubTaskID:" << sub_task_id << ",receive from master new orderTask:");

    LogOutCurOrder(order_task);
    UpdateToRcsInfo(order_task);

    TaskId taskid;
    taskid.task_id = task_id;
    taskid.subtask_id = sub_task_id;
    task_id_buffer_.AddData(&taskid, 1);
    // add new order task
    use_speed_level_ = true;
    SetLevel(9);
    order_task_buffer_.AddData(order_task);
    DEBUG_OUT("add order task is success");
    return true;
}

/**
 * @brief 更新RCS信息
 *
 * 根据传入的订单信息更新VhcStatus中的rcs_fb_信息
 *
 * @param order 订单信息
 * @return 更新成功返回true，失败返回false
 */
bool TaskManager::UpdateToRcsInfo(agv_msgs::msg::Order order)
{
    auto &info = VhcStatus.rcs_fb_;
    DEBUG_OUT("UpdateToRcsInfo:"
              << ",orderid:" << order.orderid
              << ",orderupdateid:" << order.orderupdateid);

    auto nodes = order.nodes;
    std::size_t node_count = nodes.size();
    DEBUG_OUT("UpdateToRcsInfo:nodes.size:" << (int)(nodes.size()));
    if (node_count > 0)
    {
        agv_msgs::msg::MapNode cur_node = nodes[node_count - 1];
        info.order_id = order.orderid;
        info.order_update_id = order.orderupdateid;
        info.node_id = cur_node.nodeid;
        DEBUG_OUT("UpdateToRcsInfo ok," << ",orderid:" << info.order_id
                                        << ",orderupdateid:" << info.order_update_id
                                        << ",nodeid:" << cur_node.nodeid
                                        << ",sequenceid:" << cur_node.sequenceid);
        return true;
    }

    DEBUG_ERROR_OUT("error:UpdateToRcsInfo failed:"
                    << ",orderid:" << order.orderid
                    << ",orderupdateid:" << order.orderupdateid);
    return false;
}

/**
 * @brief 设置任务管理器的速度等级
 *
 * 设置任务管理器的速度等级，并将其限制在0到9之间。
 *
 * @param level 所需设置的速度等级，应为0到9之间的整数
 */
void TaskManager::SetLevel(int level)
{
    DEBUG_OUT("set speed level = " << (int)level);

    if (level < 0)
        level = 0;
    else if (level > 9)
        level = 9;

    level_ = level;
    VhcStatus.level_ = level_;
}

/**
 * @brief 删除当前NV任务
 *
 * 删除当前非易失性存储器（NV）任务。
 *
 * 如果当前存在NV任务（即cur_nv_不为空），则执行删除操作，并输出相应的调试信息。
 */
void TaskManager::DelNvTask()
{
    if (cur_nv_) {
        DEBUG_OUT("DelNvTask,try to delete cur nv");
        delete cur_nv_;
        cur_nv_ = nullptr;
        DEBUG_OUT("DelNvTask,delete cur nv is finish");
    }
}

/**
 * @brief 删除当前操作任务
 *
 * 该函数用于删除当前正在执行的操作任务。
 * 如果当前存在操作任务，则会尝试强制停止该任务，并删除它。
 *
 * @return 无返回值
 */
void TaskManager::DelOpTask()
{
    if (cur_op_) {
        DEBUG_OUT("DelOpTask,try to force stop action");
        cur_op_->ForceStop();
        delete cur_op_;
        cur_op_ = nullptr;
        DEBUG_OUT("DelOpTask,delete cur op is finish");
    }
}

/**
 * @brief 删除动态任务
 *
 * 删除当前正在执行的动态任务。如果当前有正在执行的动态任务，则强制停止该任务并删除任务对象。
 */
void TaskManager::DelDynTask() 
{
    if (cur_dyn_op_) {
        DEBUG_OUT("DelDynTask,try to force stop dyn action");
        cur_dyn_op_->ForceStop();
        delete cur_dyn_op_;
        cur_dyn_op_ = nullptr;
        DEBUG_OUT("DelDynTask,delete cur dynop is finish");
    }
}

/**
 * @brief 清除工作缓冲区
 *
 * 清空工作缓冲区中的所有工作码，并释放它们所占用的内存。
 *
 * @note 该函数会遍历工作缓冲区中的所有工作码，根据工作码的类型释放相应的内存。
 * 如果工作码类型为NV_WORKCODE，则释放TrajNv对象所占用的内存；
 * 如果工作码类型为OP_WORKCODE或DYN_WORKCODE，则释放COperation对象所占用的内存。
 */
void TaskManager::ClearWorkBuffer()
{
    while (!work_buffer_.IsEmpty()) {
        WorkCode workcode = work_buffer_.Get();

        if (workcode.type_ == WorkCodeType::NV_WORKCODE) {
            if ((TrajNv *)(workcode.pointer_)) {
                delete (TrajNv *)(workcode.pointer_);
                workcode.pointer_ = (DWORD)nullptr;
            }
        } else if (workcode.type_ == WorkCodeType::OP_WORKCODE || workcode.type_ == WorkCodeType::DYN_WORKCODE) {
            if ((Operation *)(workcode.pointer_)) {
                delete (Operation *)(workcode.pointer_);
                workcode.pointer_ = (DWORD)nullptr;
            }
        }
    }
}

/**
 * @brief 清空工作缓冲区副本
 *
 * 清空工作缓冲区副本，但不删除其中的WorkCode对象指针，因为其与工作缓冲区中的指针相同。
 */
void TaskManager::ClearWorkBufferCopy()
{
    // FIMXE: work_buffer_copy_ can not delete WorkCode.ptr because the pointer is the same as work_buffer_
    while (!work_buffer_copy_.IsEmpty())
    {
        WorkCode wordcode = work_buffer_copy_.Get();
        (void)(wordcode);
    }
}

/**
 * @brief 清空车辆状态
 *
 * 该函数用于将车辆状态重置为初始状态。
 */
void TaskManager::ClearVhcStatus()
{
    VhcStatus.task_status_ = TaskStatus::NormalFinish;
    VhcStatus.velocity_.linear_ = 0.0f; // clear vel prog by QF
    VhcStatus.velocity_.angular_ = 0.0f;
    VhcStatus.prog_percent_ = 0.0f;
    VhcStatus.is_idle_ = true;
    VhcStatus.mode_ = PROFILE_MODE_NORMAL;
    VhcStatus.rcs_fb_.Clear();
    VhcStatus.to_run_blink_ = false;
    idle_to_run_.Reset(); // 空闲->启动 发送启动标识
    // 清除空闲状态标志
    UpdateRackFollowUp(nullptr);
}

/**
 * @brief 清除所有任务
 *
 * 该函数用于清除所有任务相关的数据，并将任务管理器重置为初始状态。
 */
void TaskManager::ClearAllTask()
{
    // std::lock_guard<std::mutex> locker(m_mutex);
    DEBUG_OUT("ClearAllTask start:");

    ClearWorkBuffer();
    ClearWorkBufferCopy();

    // #2 clear curWorkCode and task buffer
    cur_work_code_.Nullify();
    task_id_buffer_.RemoveAll();
    order_task_buffer_.RemoveAll();
    local_order_.Clear();
    // #3 clear vhicle status
    ClearVhcStatus();
    dyn_virt_status_ = VhcStatus;
    DEBUG_WARN_OUT("m_DynVirtStatus = VhcStatus; VhcStatus.m_nLocType=" << (int)VhcStatus.loc_type_
                                                                        << ",m_DynVirtStatus.m_nLocType)=" <<(int)dyn_virt_status_.loc_type_);
    task_status_  = START_WORKCODE;
    // m_nSpinTurnDir  = SMART_TURN_DIR;
    // m_AfterSpinMoveDir = FORWARD;
    can_compile_one_new_task_  = false;
    work_state_ = AGV_IS_IDLE;
    lc_end_task_ = FALSE;
    record_last_nv_vel_.SetZero();               // m_recordLastNvVel.linear_ = 0.0; m_recordLastNvVel.fAngular = 0.0;// add by QF
    cur_task_id_ = VhcStatus.task_id_ = 65535;   // add by QF
    cur_sub_task_id_ = VhcStatus.sub_task_id_ = 65535;
    suspend_for_dyn_op_ = false;
    // #1 delete curNav
    DelNvTask();
    DelOpTask();
    DelDynTask();
    ClearPidCtrl(0);
    // 清除任务时，设置规划准备
    global_plan_.target_info_.SetPlanFlag(true);
    global_plan_.plan_mode_.Reset();
    DEBUG_OUT("ClearAllTask end:");
}

/**
 * @brief 清除所有任务
 *
 * 该函数用于清除所有任务相关的数据，并将任务管理器重置为初始状态。
 */
void TaskManager::UpdateRackFollowUp(TrajNv *cur_nv)
{
    VhcStatus.rack_follow_up_ = cur_nv ? cur_nv->CanRackFollowUp() : false;
    DEBUG_OUT("UpdateRackFollowUp=" << VhcStatus.rack_follow_up_);
}

/**
 * @brief 处理任务结束的函数
 *
 * 该函数负责处理当前任务的结束，包括强制停止当前操作、更新操作列表状态、清除所有任务等操作。
 *
 * @note 该函数需要在加锁状态下执行，以避免并发问题。
 */
void TaskManager::HandleEndTask()
{
    std::lock_guard<std::mutex> locker(run_obj_mutex_);
    if (!CancelCurTask()) return;
    bool is_task_finished = false;
    cur_op_ ? cur_op_->ForceStop() : false;
    cur_dyn_op_ ? cur_dyn_op_->ForceStop() : false;

    // #1 only op
    if (cur_op_) {
        if (cur_op_->Finished()) {
            DEBUG_OUT("cur op finish; ready clear all task");
            UpdateActionListStates(SetActionState(cur_op_->GetCurActionObj(), STATUS_FAILED));
            ClearAllTask(); // handleEndTask:1
            SetCancelTask(false);
            is_task_finished = true;
        }
    }  else if (cur_dyn_op_ && !cur_nv_) {// #2 only dyn op
        bool stopped = fabsf(record_last_nv_vel_.linear_) < 0.0001f && fabsf(record_last_nv_vel_.angular_) < 0.0001f;
        if (cur_dyn_op_->Finished() && stopped) {
            DEBUG_OUT("cur dyn op finish; ready clear all task");
            UpdateActionListStates(SetActionState(cur_dyn_op_->GetCurActionObj(), STATUS_FAILED));
            ClearAllTask(); // handleEndTask:2
            SetCancelTask(false);
            is_task_finished = true;
        }
    } else if (cur_nv_) {// #3 nv + dyn op // cur_dyn_op_
        bool is_dyn_op_finished = cur_dyn_op_ ? cur_dyn_op_->Finished() : true;
        USHORT start_node = cur_nv_->start_node_id_;
        USHORT end_node = cur_nv_->end_node_id_;
        bool suspend = cur_nv_->GetWorkMode() != NV_NORMAL;
        bool stopped = fabsf(cur_nv_->GetLinearVel()) < 0.0001f && fabsf(cur_nv_->GetAngularVel()) < 0.0001f;
        if (suspend && stopped && is_dyn_op_finished) {
            if (cur_nv_->GetTrajType() == (short)TrajType::SPIN_TRAJ) {
                // FIXEM: temp is can not use
                VhcStatus.loc_type_ = TagStateType::STATIC_LOCATION; // for set static locationg VhcStatus.m_nLocType
                DEBUG_ERROR_OUT("spin traj force  cancel task;end_node=" << end_node);
            }

            DEBUG_OUT("force cancle task start_node=" << start_node << ",end_node=" << end_node);
            record_last_nv_vel_.linear_ = cur_nv_->GetLinearVel();
            record_last_nv_vel_.angular_ = cur_nv_->GetAngularVel();
            curvature_ = cur_nv_->Curvature();
            last_steer_angle_ = cur_nv_->GetSteerAngle();
            DEBUG_WARN_OUT("cur nv  suspend; ready clear all task;VhcStatus.nLocType=" << (int)VhcStatus.loc_type_
                                                                                       << ",m_DynVirtStatus.m_nLocType=" << (int)dyn_virt_status_.loc_type_
                                                                                       << ",m_lastSteerAngle=" << last_steer_angle_.radian_);
            ClearAllTask(); // handleEndTask:3
            SetCancelTask(false);
            is_task_finished = true;
            DEBUG_WARN_OUT("cur nv  suspend; clear all task finish:VhcStatus.m_nLocType=" << (int)VhcStatus.loc_type_
                                                                                          << ",m_DynVirtStatus.m_nLocType=" << (int)dyn_virt_status_.loc_type_);
        }
    } else {
        bool stopped = fabsf(record_last_nv_vel_.linear_) < 0.0001f && fabsf(record_last_nv_vel_.angular_) < 0.0001f;
        if (stopped) {
            DEBUG_OUT("cur all obj is null; ready clear all task");
            ClearAllTask(); // handleEndTask:4
            SetCancelTask(false);
            is_task_finished = true;
        }
    }

    if (is_task_finished) {
        DEBUG_OUT("bEndTaskFinish is ok;ready clear mark check buffer");
        SingleCheckObj->ClearCheckBuffer();
    } else {
        DEBUG_OUT("wait:ready clear mark check buffer");
    }
}

/**
 * @brief 当空闲时执行的操作
 *
 * 当系统处于空闲状态时，根据参数决定是否开启指示灯闪烁，并设置停止类别。
 *
 * @note 该函数在空闲时自动调用。
 */
void TaskManager::WhenIdle()
{
    auto p = MotionParm::read_to_run_delay_params_;
    // 如果不开启
    if (!p.use) {
        VhcStatus.to_run_blink_ = false;
        SetStopCategory(StopType::STOP_START_DELAY, VhcStatus.to_run_blink_);
        return;
    }

    // 当前时op模式，认为是空闲
    bool op = AGV_IS_IN_OP == work_state_;
    // 正常开启
    bool is_idle = op ? true : IsIdle();
    VhcStatus.to_run_blink_ = idle_to_run_.Update(is_idle, p.idle_time_ms, p.delay_time_ms);
    SetStopCategory(StopType::STOP_START_DELAY, VhcStatus.to_run_blink_);
}

/**
 * @brief 空闲时触发错误
 *
 * 如果当前处于空闲状态并且需要触发错误，则触发错误并返回true；
 * 否则，仅记录错误但不触发，并返回false。
 *
 * @param occur 是否需要触发错误
 * @param msg 错误信息
 * @return 是否成功触发错误
 */
bool TaskManager::IdleTryToOccurError(bool occur, const std::string &msg)
{
    bool ret = false;
    bool is_idle = IsIdle();
    if (is_idle && occur) {
        SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_TASK_COMPILE_ERROR), msg, agv_msgs::msg::AGVEvent::ALARM, true);
        ret = true;
        DEBUG_ERROR_OUT("IdleTryToOccurError true");
    } else {
        SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_TASK_COMPILE_ERROR), msg, agv_msgs::msg::AGVEvent::NONE, false);
    }
    return ret;
}

void TaskManager::OtherDoingProc()
{
}

/**
 * @brief 执行预处理操作
 *
 * 在执行主要任务之前，调用此方法执行一些预处理操作。
 */
void TaskManager::PreDoingProc()
{
    WhenIdle();
}

/**
 * @brief 判断是否可以进行新的规划
 *
 * 该函数用于判断当前是否可以重新进行规划。
 *
 * @return 如果可以重新规划，则返回true；否则返回false。
 */
bool TaskManager::CanRePlan()
{
    // 如果规划记录已经规划过了，则跳过新的规划
    if (!global_plan_.target_info_.NeedPlan()) {
        DEBUG_OUT("can not need new global plan");
        return false;
    }

    // 如果当前正在运行--静态动作，停止新规划
    if (cur_op_) {
        DEBUG_OUT("can not need CurvePlan; curOp is running");
        return false;
    }

    // 如果队列为空且当前无运行的行走任务(空闲)
    bool is_empty = work_buffer_.GetCount() == 0;
    if (is_empty && !cur_nv_) {
        DEBUG_WARN_OUT("RunCurvePlan: workbuffer is empty;cur nv is nullptr");
        return false;
    }

    //  如果当前正在运行--动态动作，暂时忽略
    // if (cur_dyn_op_)
    // {
    //     DEBUG_OUT("CurvePlan normal; cur DynOp is running");
    // }
    return true;
}

/**
 * @brief 选择速度加载参数组
 *
 * 根据当前加载状态选择相应的速度加载参数组。
 *
 * @return 返回选定的速度加载参数组
 */
CurveParamGroup TaskManager::SelectVelLoad()
{
    // 运动参数
    auto param = MotionParm::curve_params_;
    bool load_flag = MotionParm::load_flags_.current_load;
    auto vel_group = load_flag ? param.load : param.unload;
    DEBUG_OUT("RunCurvePlan: loadFlag=" << load_flag);
    return vel_group;
}

/**
 * @brief 负责重新规划曲线任务的逻辑。
 * 函数RunCurvePlan()检查是否可以重新规划，并根据当前的运行模式和任务状态执行不同的重新规划策略。
 * 主要处理自旋任务和非自旋任务的规划逻辑，包括调整速度、距离等参数。
 * 在规划过程中，函数考虑了不同的运行模式(PROFILE_MODE_REFRAIN和PROFILE_MODE_SUSPEND)对规划策略的影响。
 * 代码还处理了任务缓冲区中的任务队列，确定是否需要将当前任务与下一个任务连接起来。
 * 在整个过程中，函数会输出调试信息，以便于跟踪规划过程的执行。
 */
void TaskManager::RunCurvePlan()
{
    if (!CanRePlan()) return;
    // 有无货物的速度参数
    auto load_vel_params = SelectVelLoad();

    // #1 如果当前时路径任务，如果是自旋，则跳过新的规划
    // 当前的速度，当前的距离，当前路段总共的距离
    // 获取当前的运行速度,都是绝对值
    float cur_vel = fabs(record_last_nv_vel_.linear_), cur_w = fabs(record_last_nv_vel_.angular_), cur_q = 0.0f, range = 0.0f, last_max_vel = 0.0f;
    float last_end_value = 0;
    bool first = true;

    // 当前记录的信息-用来计算速度不均匀时，上一次的规划终点的信息。
    PlanTargetInfo::DataType cursor_data_info;

    // #2 根据当前操作模式，选择不同的重规划方式
    auto &run_mode = global_plan_.plan_mode_;
    DEBUG_OUT("enter new plan;mode=" << run_mode.Mode());

    if (cur_nv_) {
        // #1 自旋重规划，只关心当前自旋的一个阶段
        bool spin = cur_nv_->GetTrajType() == (short)TrajType::SPIN_TRAJ;
        cur_vel = fabs(cur_nv_->GetCurValue());
        cur_w = fabs(cur_nv_->GetAngularVel());
        cur_q = cur_nv_->GetRelProgress();
        range = cur_nv_->GetTraj()->GetRange();
        last_max_vel = fabs(cur_nv_->GetMaxVel());
        // 如果是直线停车
        last_end_value = cur_nv_->GetEndValue();
        bool cur_stop = fabs(last_end_value) < 0.0001f;
        // 更新规划目标信息
        SHORT type = cur_nv_->GetTraj()->type_;
        auto start_id = cur_nv_->start_node_id_;
        auto end_id = cur_nv_->end_node_id_;

        cursor_data_info.Update(type, start_id, end_id, last_end_value);
        if (spin) {
            // 如果是自旋中间被打断，则重新规划
            DEBUG_WARN_OUT("RunCurvePlan: spin traj replan; cur nv is running");
            // 系统参数
            double vmax = std::min((double)(last_max_vel), load_vel_params.spin.vmax);
            double amax = load_vel_params.spin.amax, jmax = load_vel_params.spin.jmax;
            double q0 = cur_q, q1 = range, v0 = cur_w, v1 = 0;

            // #1 切入当前的运行控制模式
            switch (run_mode.Mode()) {
                case PROFILE_MODE_REFRAIN:
                {
                    DEBUG_WARN_OUT("CurvePlan:curSpin refranin vmax=" << vmax
                                                                    << ",run_mode.vmax="
                                                                    << run_mode.vmax);
                    vmax = std::min(vmax, run_mode.vmax);
                    // 如果起点速度高于抑制最大速度，则分两次规划
                    RefrainDivide(v0, v1, vmax, amax, jmax, q0, q1, TO_RADIAN(20));
                    // 控制到路径点后，自动复位重规划标识
                    global_plan_.target_info_.Update(type, start_id, end_id, v1);
                    DEBUG_OUT("spin:refrain_mode:std::max(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                    vmax = std::max(vmax, v0);
                }
                break;
                case PROFILE_MODE_SUSPEND:
                {
                    // 自旋缓停逻辑待测试
                    // FIXME:当前直线等缓停逻辑补充
                    // 初始值，是当前路段的距离
                    float total = range;                                                   // 规划时总共路线的距离
                    double cur_dec_q = global_plan_.DisRequirement(false, cur_w, 0, amax, jmax); // dec减速距离
                    DEBUG_OUT("RunCurvePlan curSpin readyStop: curDecQ=" << cur_dec_q << ",fCurW=" << cur_w);
                    bool has_q = total >= (cur_dec_q + cur_q); // 距离已经够用
                    if (has_q) {
                        // 如果停车距离几乎接近于目标停车点，则直接规划到目标点即可
                        float diff_q = total - (cur_dec_q + cur_q);
                        if (diff_q > 0 && diff_q < TO_RADIAN(20)) {
                            // total = total;
                            DEBUG_OUT("RunCurvePlan:curSpin:forceStop: suspend_mode:total nearly equal curDecQ + fCurQ"
                                      << ",fCurQ=" << cur_q
                                      << ",curTotal=" << total
                                      << ",curDecQ=" << cur_dec_q);
                        } else {
                            DEBUG_OUT("RunCurvePlan:curSpin:forceStop: suspend_mode" << ",fCurQ=" << cur_q
                                                                                     << ",curTotal=" << total
                                                                                     << ",curDecQ=" <<  cur_dec_q
                                                                                     << ",useTotal=" << cur_dec_q + cur_q);
                            total = cur_dec_q + cur_q;
                        }

                        DEBUG_OUT("RunCurvePlan:curSpin:forceStop: suspend_mode" << ",q1Old=" << q1 << ",set q1=" << total);
                        q1 = total;
                    }
                    // FXIME:缓停模式：直至停下来，不自动进行下一次启动规划
                    global_plan_.target_info_.Clear(); // 强制清除目标信息，不在自动释放规划标识
                    DEBUG_OUT("CurvePlan: curSpin slow stop force clear targetInfo");
                }
                break;
                default:
                {
                    // 控制到路径点后，自动复位重规划标识
                    global_plan_.target_info_.Update(type, start_id, end_id, v1);
                }
                break;
            }

            // FIXME: v0必须 <=vmax
            DEBUG_OUT("spin:std::min(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
            v0 = std::min(v0, vmax);
            // #2 开始新的全速度局规划
            StartNewGlobalPlan(vmax, amax, jmax, GetCycle(), q0, q1, v0, v1);
            global_plan_.target_info_.SetPlanFlag(false);
            DEBUG_OUT("CurvePlan: cur reload spin traj: global plan:"
                      << ",vmax=" << vmax
                      << ",amax=" << amax
                      << ",jmax=" << jmax
                      << ",q0=" << q0 << ",q1=" << q1
                      << ",v0=" << v0 << ",v1=" << v1
                      << ",type=" << type << ",end_id=" << start_id << ",end_id=" << end_id);

            return;
        } else if (cur_stop) {
            //  序列为空时，结束速度一定为0(v1=0)
            //  只重新规划当前路径
            //  如果是自旋中间被打断，则重新规划
            DEBUG_WARN_OUT("RunCurvePlan: other traj replan; cur nv is running");
            // 系统参数
            double vmax = std::min((double)(last_max_vel), load_vel_params.line.vmax);
            double amax = load_vel_params.line.amax, jmax = load_vel_params.line.jmax;
            double q0 = cur_q, q1 = range, v0 = cur_vel, v1 = 0;

            // #1 切入当前的运行控制模式
            switch (run_mode.Mode()) {
                case PROFILE_MODE_REFRAIN:
                {
                    DEBUG_WARN_OUT("CurvePlan:curNv refranin vmax=" << vmax
                                                                    << ",run_mode.vmax="
                                                                    << run_mode.vmax);
                    vmax = std::min(vmax, run_mode.vmax);

                    // 如果起点速度高于抑制最大速度，则分两次规划
                    RefrainDivide(v0, v1, vmax, amax, jmax, q0, q1, 0.15);
                    global_plan_.target_info_.Update(type, start_id, end_id, v1);
                    DEBUG_OUT("cur_stop:refrain_mode:std::max(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                    vmax = std::max(vmax, v0);
                }
                break;
                case PROFILE_MODE_SUSPEND:
                {
                    // 自旋缓停逻辑待测试
                    // FIXME:当前直线等缓停逻辑补充
                    // 初始值，是当前路段的距离
                    float total = range;                                                   // 规划时总共路线的距离
                    double cur_dec_q = global_plan_.DisRequirement(false, cur_w, 0, amax, jmax); // dec减速距离
                    DEBUG_OUT("RunCurvePlan curSpin readyStop: curDecQ=" << cur_dec_q << ",fCurW=" << cur_w);
                    bool has_q = total >= (cur_dec_q + cur_q); // 距离已经够用
                    if (has_q) {
                        // 如果停车距离几乎接近于目标停车点，则直接规划到目标点即可
                        float diff_q = total - (cur_dec_q + cur_q);
                        if (diff_q > 0 && diff_q < TO_RADIAN(20)) {
                            // total = total;
                            DEBUG_OUT("RunCurvePlan:curSpin:forceStop: suspend_mode:total nearly equal curDecQ + fCurQ"
                                      << ",fCurQ=" << cur_q
                                      << ",curTotal=" << total
                                      << ",curDecQ=" << cur_dec_q);
                        } else {
                            DEBUG_OUT("RunCurvePlan:curSpin:forceStop: suspend_mode" << ",fCurQ=" << cur_q
                                                                                     << ",curTotal=" << total
                                                                                     << ",curDecQ=" <<  cur_dec_q
                                                                                     << ",useTotal=" << cur_dec_q + cur_q);
                            total = cur_dec_q + cur_q;
                        }

                        DEBUG_OUT("RunCurvePlan:curSpin:forceStop: suspend_mode" << ",q1Old=" << q1 << ",set q1=" << total);
                        q1 = total;
                    }
                    // FXIME:缓停模式：直至停下来，不自动进行下一次启动规划
                    global_plan_.target_info_.Clear(); // 强制清除目标信息，不在自动释放规划标识
                    DEBUG_OUT("CurvePlan: curNv slow stop force clear targetInfo");
                }
                break;
                default:
                {
                    // 控制到路径点后，自动复位重规划标识
                    global_plan_.target_info_.Update(type, start_id, end_id, v1);
                }
                break;
            }

            // FIXME: v0必须 <=vmax
            DEBUG_OUT("cur_stop:std::min(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
            v0 = std::min(v0, vmax);
            // 开始新的全速度局规划
            StartNewGlobalPlan(vmax, amax, jmax, GetCycle(), q0, q1, v0, v1);
            global_plan_.target_info_.SetPlanFlag(false);
            DEBUG_OUT("CurvePlan: cur reload other traj: global plan:"
                      << ",vmax=" << vmax
                      << ",amax=" << amax
                      << ",jmax=" << jmax
                      << ",q0=" << q0 << ",q1=" << q1
                      << ",v0=" << v0 << ",v1=" << v1
                      << ",type=" << type << ",end_id=" << start_id << ",end_id=" << end_id);
            return;
        } else {
            first = false;
            DEBUG_OUT("cur nv continue re plan:"
                      << ",fCurVel=" << cur_vel << ",fCurQ=" << cur_q
                      << ",fRange=" << range << ",last_max_vel=" << last_max_vel
                      << ",fLastEndValue=" << last_end_value
                      << ",type=" << type << ",start_id=" << start_id
                      << ",end_id=" << end_id);
        }
    }

    // 初始值，是当前路段的距离
    float total = range;              // 规划时总共路线的距离
    // float  endVelRecord = max_vel; // 规划时，上一次路线结束时的速度值，如果不相同，则需要重新规划
    double dec_q = global_plan_.DisRequirement(false, cur_vel, 0); // decQ、dec_q2验证两种减速距离计算是否相同
    double dec_q2 = global_plan_.GetStopDistance(cur_vel);
    DEBUG_OUT("RunCurvePlan: decQ=" << dec_q << ",dec_q2=" << dec_q2 << ",cur nv fCurVel=" << cur_vel);

    ////////////开启遍历任务缓冲区//////////////////////
    USHORT next_pos = work_buffer_.HeadPos(); // #3-2 查找队列头数据是否存在;如果是直线，曲线，侧移等，看是否需要和下一个任务串起来
    WorkCode *next_work_code = work_buffer_.PeekItem(next_pos);
    if (!next_work_code) {
        DEBUG_ERROR_OUT("error:RunCurvePlan: next is null");
        return;
    }

    while (next_work_code) {
        WorkCodeType type = next_work_code->type_;
        switch (type) {
            case WorkCodeType::NV_WORKCODE:
            {
                // #2 正常逻辑，工作码有效且是导航码
                DEBUG_WARN_OUT("RunCurvePlan: find nv_code ok once");
                // 开始准备，累加路径长度
                TrajNv* next_nv = (TrajNv *)(next_work_code->pointer_);
                if (next_nv) {
                    Trajectory* traj = next_nv->GetTraj();
                    SHORT next_type = traj->type_;
                    // fixbug:自旋时，使用当前位姿计算角度差
                    if ((short)TrajType::SPIN_TRAJ == next_type) {
                        DEBUG_OUT("spin_traj,TryModifySpinDir start");
                        // 是否根据实际起始角度 算出自旋的方向
                        TryModifySpinDir(next_nv->GetTraj());
                        DEBUG_OUT("spin_traj,TryModifySpinDir end");
                    }
                    auto next_start_id = next_nv->start_node_id_;
                    auto next_end_id = next_nv->end_node_id_;
                    auto next_max_vel = next_nv->GetMaxVel();
                    auto next_end_vel = next_nv->GetEndValue();
                    auto next_range = traj->GetRange();
                    DEBUG_OUT("RunCurvePlan:nextType=" << next_type << ",next_start_id=" << next_start_id << ",next_end_id=" << next_end_id
                                                       << ",next_max_vel=" << next_max_vel << ",next_end_vel=" << next_end_vel << ",next_range=" << next_range);
                    switch (next_type) {
                        case (short)TrajType::UNDEFINED_TRAJ:
                        {
                            DEBUG_ERROR_OUT("error: RunCurvePlan, unknown traj");
                            return;
                        }
                        break;
                        case (short)TrajType::LINE_TRAJ:
                        case (short)TrajType::BEZIER_TRAJ:
                        case (short)TrajType::SIDE_TRAJ:
                        {
                            // 如果是轨迹停车
                            bool ready_stop = fabs(next_end_vel) < 0.0001f;
                            // 如果是第一条轨迹，需要使用当前的最大速度赋初值
                            if (first) {
                                first = false;
                                last_max_vel = next_max_vel;
                                DEBUG_OUT("CurvePlan stop: first line traj; last_max_vel=" << last_max_vel << ",continue");
                            }
                            //////////////////////如果是缓停模式//////////////////////////
                            bool slowStop = run_mode.Mode() == PROFILE_MODE_SUSPEND;
                            if (slowStop) {
                                // 加上新的路径长度
                                total += next_range;
                                DEBUG_OUT("CurvePlan: total = " << total << ",add newCurSize=" << next_range);
                                bool has_q = total >= (dec_q + cur_q); // 距离已经够用
                                // 如果减速距离足够 || 已经准备停车了->则必须要截断当前规划了
                                if (has_q || ready_stop) {
                                    // 如果停车距离几乎接近于目标停车点，则直接规划到目标点即可
                                    float diff_q = total - (dec_q + cur_q);
                                    if (diff_q > 0 && diff_q < 0.3) {
                                        total = total;
                                        DEBUG_OUT("forceStop: CurvePlan PROFILE_MODE_SUSPEND:total nearly equal dec_q + fCurQ");
                                    } else {
                                        DEBUG_OUT("forceStop:CurvePlan PROFILE_MODE_SUSPEND" << ",cur_q=" << cur_q
                                                                                             << ",total=" << total
                                                                                             << ",dec_q=" << dec_q
                                                                                             << ",latertotal=" << dec_q + cur_q);
                                        total = dec_q + cur_q;
                                    }
                                    // FIXME:可能会出现当前速度时0，减速距离则是0；规划失败
                                    DEBUG_OUT("CurvePlan: forceStop last; next_max_vel=" << next_max_vel
                                                                                       << ",newEndVel set 0;<<newtotal=" << total
                                                                                       << ",dec_q=" << dec_q
                                                                                       << ",fCurQ=" << cur_q);
                                    // 系统参数
                                    double vmax = cur_vel; // std::min((double)endVelRecord, velLoad.line.vmax);
                                    double amax = load_vel_params.line.amax, jmax = load_vel_params.line.jmax;
                                    double q0 = cur_q, q1 = total, v0 = cur_vel, v1 = 0;

                                    // FIXME: v0必须 <=vmax
                                    DEBUG_OUT("slow_stop:std::min(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                                    v0 = std::min(v0, vmax);
                                    StartNewGlobalPlan(vmax, amax, jmax, GetCycle(), q0, q1, v0, v1);
                                    global_plan_.target_info_.SetPlanFlag(false);
                                    global_plan_.target_info_.Clear(); // 强制清除目标信息，不在自动释放规划标识

                                    DEBUG_OUT("CurvePlan:  slow stop force clear targetInfo;line traj force set stop: global plan:"
                                              << ",vmax=" << vmax
                                              << ",amax=" << amax
                                              << ",jmax=" << jmax
                                              << ",total=" << total
                                              << ",dec_q=" << dec_q
                                              << ",q0=" << q0 << ",q1=" << q1
                                              << ",v0=" << v0 << ",v1=" << v1
                                              << ",nextType=" << next_type << ",next_start_id=" << next_start_id << ",next_end_id=" << next_end_id);
                                    return;
                                }
                            } else {
                                //////////////////////如果不是缓停模式////////////////////////
                                // 加上新的路径长度
                                total += next_range;
                                DEBUG_OUT("CurvePlan: total = " << total << ",add next_range=" << next_range);
                                // 规划速度不停车: 不管轨迹接续，只关心速度是否均衡
                                bool vel_match = fabs(last_max_vel - next_max_vel) < 0.01f; // 第一次的速度记录即是整个巡航路段的速度，不需要更新
                                // FIXEBUG:补充逻辑 当速度不匹配的时候，规划切断，但距离如果过小，则忽略，将当前微小距离归为下一段上
                                float distance = total - next_range;
                                float diffq0q1 = distance - cur_q; // q0q1 最小位移。如果小于10cm,归为下一路段
                                bool too_small_distance = diffq0q1 <= 0.1;
                                bool force_vel_match = !vel_match && too_small_distance && ready_stop; // 速度不匹配 && 位移过小&&准备停车->进行速度合并停车

                                if (vel_match || force_vel_match) {
                                    // 记录一下当前的速度值，继续查找
                                    DEBUG_OUT("CurvePlan: velMatch ok,continue;Update cursorDataInfo once");
                                    cursor_data_info.Update(next_type, next_start_id, next_end_id, next_end_vel);
                                    last_end_value = next_end_vel;
                                    last_max_vel = next_max_vel; // important:#1 forceVelMatch:fLastMaxVel != nextMaxVel;#2 velMatch:fLastMaxVel==velMatch;
                                    if (ready_stop) {
                                        DEBUG_OUT("CurvePlan agv ready stop: total = " << total);
                                        // 系统参数
                                        double vmax = std::min((double)(last_max_vel), load_vel_params.line.vmax); // 会修改
                                        double amax = load_vel_params.line.amax, jmax = load_vel_params.line.jmax;        // 不修改
                                        double q0 = cur_q, q1 = total, v0 = cur_vel, v1 = 0;              // 会修改
                                        if (run_mode.Mode() == PROFILE_MODE_REFRAIN) {
                                            DEBUG_WARN_OUT("CurvePlan:nextNv refranin vmax=" << vmax
                                                                                             << ",run_mode.vmax="
                                                                                             << run_mode.vmax);
                                            vmax = std::min(vmax, run_mode.vmax);
                                            // 如果起点速度高于抑制最大速度，则分两次规划
                                            RefrainDivide(v0, v1, vmax, amax, jmax, q0, q1, 0.15);
                                            DEBUG_OUT("velMatch:refrain_mode:std::max(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                                            vmax = std::max(vmax, v0);
                                        }

                                        // FIXME: v0必须 <=vmax
                                        DEBUG_OUT("velmatch:std::min(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                                        v0 = std::min(v0, vmax);
                                        //  开始新的全速度局规划
                                        StartNewGlobalPlan(vmax, amax, jmax, GetCycle(), q0, q1, v0, v1);
                                        global_plan_.target_info_.SetPlanFlag(false);
                                        auto p = cursor_data_info;
                                        global_plan_.target_info_.Update(p.traj_type, p.start_node, p.end_node, v1);
                                        DEBUG_OUT("CurvePlan line traj stop: global plan:"
                                                  << ",vmax=" << vmax
                                                  << ",amax=" << amax
                                                  << ",jmax=" << jmax
                                                  << ",total=" << total
                                                  << ",q0=" << q0 << ",q1=" << q1
                                                  << ",v0=" << v0 << ",v1=" << v1
                                                  << ",type=" << p.traj_type << ",start_id=" << p.start_node
                                                  << ",end_id=" << p.end_node);
                                        return;
                                    }
                                } else {
                                    // 当速度不匹配的时候，规划切断，但距离如果过小，则忽略，将当前微小距离归为下一段上
                                    // float dis = total - next_range;
                                    // float diffq0q1 = dis - cur_q; // q0q1 最小位移。如果小于10cm,归为下一路段
                                    if (too_small_distance && !ready_stop) {

                                        DEBUG_WARN_OUT("Warining CurvePlan: vel not Match; force ignore cur small "
                                                       << ",dis=" << distance
                                                       << ",diffq0q1=" << diffq0q1
                                                       << ",last_max_vel=" << last_max_vel
                                                       << "next_max_vel=" << next_max_vel
                                                       << ",fLastEndValue=" << last_end_value
                                                       << ",next_end_vel=" << next_end_vel
                                                       << ",total=" << total);

                                        // 记录一下当前的速度值，继续查找
                                        DEBUG_OUT("warning:CurvePlan:continue;update cursorDataInfo once");
                                        last_end_value = next_end_vel;
                                        last_max_vel = next_max_vel;
                                        cursor_data_info.Update(next_type, next_start_id, next_end_id, next_end_vel);
                                    } else {
                                        DEBUG_OUT("CurvePlan: vel not Match; " << "next_max_vel=" << next_max_vel
                                                                               << ",last_max_vel=" << last_max_vel
                                                                               << ",fLastEndValue=" << last_end_value
                                                                               << ",total=" << total
                                                                               << ",tooSmallDis=" << too_small_distance
                                                                               << ",readyStop=" << ready_stop);

                                        // 系统参数
                                        // 特别注意：当速度不均匀时，当前循环结束，实际是规划到上一段路径的终点。
                                        double vmax = std::min((double)(last_max_vel), load_vel_params.line.vmax);
                                        double amax = load_vel_params.line.amax, jmax = load_vel_params.line.jmax;
                                        double q0 = cur_q, q1 = distance, v0 = cur_vel, v1 = last_end_value;
                                        if (run_mode.Mode() == PROFILE_MODE_REFRAIN) {
                                            DEBUG_WARN_OUT("CurvePlan:vel not Match  refranin vmax=" << vmax
                                                                                                     << ",run_mode.vmax="
                                                                                                     << run_mode.vmax
                                                                                                     << ",fLastEndValue=" << last_end_value);
                                            vmax = std::min(vmax, run_mode.vmax);
                                            v1 = std::min((double)v1, run_mode.vmax);

                                            // 如果起点速度高于抑制最大速度，则分两次规划
                                            RefrainDivide(v0, v1, vmax, amax, jmax, q0, q1, 0.15);
                                            DEBUG_OUT("velNotMatch:refrain_mode:std::max(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                                            vmax = std::max(vmax, v0);
                                        }

                                        // FIXME: v0必须 <=vmax
                                        DEBUG_OUT("velNotMatch:std::min(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                                        v0 = std::min(v0, vmax);
                                        // 开始新的全速度局规划
                                        StartNewGlobalPlan(vmax, amax, jmax, GetCycle(), q0, q1, v0, v1);
                                        global_plan_.target_info_.SetPlanFlag(false);
                                        auto p = cursor_data_info;
                                        global_plan_.target_info_.Update(p.traj_type, p.start_node, p.end_node, v1);
                                        DEBUG_OUT("CurvePlan: line traj velMatch: global plan:"
                                                  << ",vmax=" << vmax
                                                  << ",amax=" << amax
                                                  << ",jmax=" << jmax
                                                  << ",total=" << total
                                                  << ",q0=" << q0 << ",q1=" << q1
                                                  << ",v0=" << v0 << ",v1=" << v1
                                                  << ",type=" << p.traj_type << ",start_id=" << p.start_node << ",end_id=" << p.end_node);
                                        return;
                                    }
                                }
                            }
                        }
                        break;
                        case (short)TrajType::SPIN_TRAJ:
                        {
                            // 一条完整的自旋规划
                            double vmax = std::min((double)(next_max_vel), load_vel_params.spin.vmax);
                            double amax = load_vel_params.spin.amax, jmax = load_vel_params.spin.jmax;
                            double q0 = 0, q1 = next_range, v0 = 0, v1 = 0;

                            // #1 切入当前的运行控制模式
                            switch (run_mode.Mode()) {
                                case PROFILE_MODE_REFRAIN:
                                {
                                    DEBUG_WARN_OUT("CurvePlan:nextSpin refranin vmax=" << vmax
                                                                                    << ",run_mode.vmax="
                                                                                    << run_mode.vmax);
                                    vmax = std::min(vmax, run_mode.vmax);
                                    // 如果起点速度高于抑制最大速度，则分两次规划
                                    RefrainDivide(v0, v1, vmax, amax, jmax, q0, q1, TO_RADIAN(20));
                                    global_plan_.target_info_.Update(next_type, next_start_id, next_end_id, v1);
                                    DEBUG_OUT("spin:refrain_mode:std::max(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                                    vmax = std::max(vmax, v0);
                                }
                                break;
                                case PROFILE_MODE_SUSPEND:
                                {
                                    // 自旋缓停逻辑待测试
                                    // FIXME: 待完善缓停逻辑
                                    DEBUG_OUT("spin: start suspend; force clear q1 is 0; q1Old=" << next_range);
                                    q1 = 0;
                                    global_plan_.target_info_.Clear(); // 强制清除目标信息，不在自动释放规划标识
                                    DEBUG_OUT("CurvePlan: nextSpin:slow stop force clear targetInfo");
                                }
                                break;
                                default:
                                {
                                    // 控制到路径点后，自动复位重规划标识
                                    global_plan_.target_info_.Update(next_type, next_start_id, next_end_id, v1);
                                }
                                break;
                            }

                            // FIXME: v0必须 <=vmax
                            DEBUG_OUT("spin:std::min(v0,vmax);v0=" << v0 << ",vmax=" << vmax);
                            v0 = std::min(v0, vmax);
                            // 开始新的全速度局规划
                            StartNewGlobalPlan(vmax, amax, jmax, GetCycle(), q0, q1, v0, v1);
                            global_plan_.target_info_.SetPlanFlag(false);
                            DEBUG_OUT("CurvePlan: spin traj: global plan:"
                                      << ",vmax=" << vmax
                                      << ",amax=" << amax
                                      << ",jmax=" << jmax
                                      << ",q0=" << q0 << ",q1=" << q1
                                      << ",v0=" << v0 << ",v1=" << v1
                                      << ",nextType=" << (int)type << ",next_start_id=" << next_start_id << ",next_end_id=" << next_end_id);
                            return;
                        }
                        break;
                        default:
                        {
                            DEBUG_ERROR_OUT("error: default RunCurvePlan, unknown traj");
                            return;
                        }
                        break;
                    }
                }
            }
            break; // end

            case WorkCodeType::NO_WORKCODE:
            {
                DEBUG_ERROR_OUT("CurvePlan errro: first traj is no_workcode"); // #1 异常情况，工作码无效，正常此逻辑不会进入
                return;
            }
            break;

            case WorkCodeType::OP_WORKCODE:
            {
                DEBUG_WARN_OUT("CurvePlan: next head code is op, can not need plan"); // 静态动作，原地不动
                return;
            }
            break;

            case WorkCodeType::DYN_WORKCODE:
            {
                // 动态动作，忽略，进行下一路段查找
                DEBUG_WARN_OUT("CurvePlan: next head code is dyn op, jump this code,continue...");
            }
            break;
        }

        next_pos = work_buffer_.NextEntry(next_pos);
        next_work_code = work_buffer_.PeekItem(next_pos);
    }

    DEBUG_WARN_OUT("CurvePlan: other error case");
}

/**
 * @brief 启动一个新的全局规划
 *
 * 该函数用于启动一个新的全局规划任务，设置最大速度、最大加速度、最大加加速度和周期等参数，并调用全局规划器进行规划。
 *
 * @param vmax 最大速度
 * @param amax 最大加速度
 * @param jmax 最大加加速度
 * @param cycle 周期
 * @param q0 起始位置
 * @param q1 目标位置
 * @param v0 起始速度
 * @param v1 目标速度
 */
void TaskManager::StartNewGlobalPlan(double vmax, double amax, double jmax, double cycle,
                                      double q0, double q1, double v0, double v1)
{

    if (vmax < 0.0001 || amax < 0.0001 || jmax < 0.0001 || cycle < 0.00001)
    {
        DEBUG_ERROR_OUT("error:StartNewGlobalPlan failed sysparam error;vmax=" << vmax
                                                                               << ",amaax=" << amax
                                                                               << ",jmax=" << jmax
                                                                               << ",cycle=" << cycle);
        // return;
    }

    // 系统参数
    global_plan_.SetSysMotionPara(vmax, amax, jmax);
    global_plan_.SetCycle(cycle);
    global_plan_.Plan(q0, q1, v0, v1);
}

/**
 * @brief 获取任务管理器的一个周期时间
 *
 * 获取任务管理器的周期时间，单位是秒。
 *
 * @return 返回任务管理器的周期时间，以秒为单位。
 */
float TaskManager::GetCycle()
{
    return 0.05f;
}

/**
 * @brief 检查任务连接状态
 *
 * 该函数用于检查当前任务连接状态，并决定是否重新规划任务。
 *
 * 1. 首先，检查规划目标信息是否有效。
 * 2. 如果当前有正在运行的静态动作，则不进行新规划。
 * 3. 如果工作缓冲区为空且当前没有运行的行走任务，则不进行规划。
 * 4. 如果当前有正在运行的行走任务，检查其速度规划目标信息是否与全局规划目标信息一致。
 *    - 如果一致，检查节点速度是否改变，如果改变则重新规划。
 *    - 如果不一致，则不进行规划。
 * 5. 如果当前没有正在运行的行走任务，则查找工作缓冲区中的规划目标点。
 *    - 如果找到符合全局规划目标信息的规划目标点，则检查节点速度是否改变，如果改变则重新规划。
 *    - 如果没有找到符合的规划目标点，则不进行规划。
 */
void TaskManager::CheckTaskConnect()
{
    // 规划目标信息有效
    PlanTargetInfo::DataType info;
    if (!global_plan_.target_info_.GetInfo(info)) {
        DEBUG_OUT("CheckTaskConnect: get info invalid");
        return;
    }

    // #1 如果当前正在运行--静态动作，停止新规划
    if (cur_op_) {
        DEBUG_OUT("CheckTaskConnect: curOp is running");
        return;
    }

    // #2 如果队列为空且当前无运行的行走任务(空闲)
    bool empty = work_buffer_.GetCount() == 0;
    if (empty && !cur_nv_) {
        DEBUG_OUT("CheckTaskConnect: workbuffer is empty;cur nv is nullptr");
        return;
    }

    // #3 是否修改当前正在运行的路段速度
    if (cur_nv_) {
        info.end_value = cur_nv_->GetEndValue();
        info.traj_type = cur_nv_->GetTraj()->type_;
        info.start_node = cur_nv_->start_node_id_;
        info.end_node = cur_nv_->end_node_id_;
        // 当前全局速度规划目标信息符合
        if (global_plan_.target_info_.Same(info)) {
            // 由于任务接续，修改过目标的节点速度不为0，则需要重新规划
            bool replan = global_plan_.target_info_.StopPointChange(info);
            if (replan)
            {
                global_plan_.target_info_.SetPlanFlag(true);
                DEBUG_OUT("CheckTaskConnect: curNv replan once");
            }
            return;
        }
    }

    // #4 查找缓冲区中的规划目标点
    USHORT next_pos = work_buffer_.HeadPos();
    WorkCode *next_work_code = work_buffer_.PeekItem(next_pos);
    while (next_work_code) {
        WorkCodeType type = next_work_code->type_;
        if (type == WorkCodeType::NV_WORKCODE) {
            DEBUG_WARN_OUT("CheckTaskConnect: find nv_code ok once");
            TrajNv *next_nv = (TrajNv *)(next_work_code->pointer_);
            PlanTargetInfo::DataType info;
            if (next_nv) {
                Trajectory *traj = next_nv->GetTraj();
                info.end_value = next_nv->GetEndValue();
                info.traj_type = traj->type_;
                info.start_node = next_nv->start_node_id_;
                info.end_node = next_nv->end_node_id_;
                // 当前全局速度规划目标信息符合
                if (global_plan_.target_info_.Same(info)) {
                    // 由于任务接续，修改过目标的节点速度不为0，则需要重新规划
                    bool replan = global_plan_.target_info_.StopPointChange(info);
                    if (replan) {
                        global_plan_.target_info_.SetPlanFlag(true);
                        DEBUG_OUT("CheckTaskConnect: replan once");
                    }
                    return;
                }
            }
        }
        next_pos = work_buffer_.NextEntry(next_pos);
        next_work_code = work_buffer_.PeekItem(next_pos);
    }
}

/**
 * @brief 抑制分段函数
 *
 * 如果起点速度高于抑制最大速度，则分两次规划。
 *
 * @param v0 起点速度
 * @param v1 终点速度（引用传递）
 * @param vmax 抑制最大速度
 * @param amax 最大加速度
 * @param jmax 最大加加速度
 * @param q0 起点位置
 * @param q1 终点位置（引用传递）
 * @param minQ 最小距离
 */
void TaskManager::RefrainDivide(double v0, double &v1, double vmax, double amax, double jmax, double q0, double &q1, double minQ)
{
    // 如果起点速度高于抑制最大速度，则分两次规划
    if (v0 - vmax > 0.0001) {
        double dec_q = global_plan_.DisRequirement(false, v0, vmax, amax, jmax);
        double q1q0 = q1 - q0;
        bool valid = (q1q0 - dec_q) >= minQ; // 如果剩余距离足够，则分段
        double q1Old = q1;                  // 仅仅记录使用
        double v1Old = v1;
        if (valid) {
            q1 = q0 + dec_q;
            v1 = vmax;
            global_plan_.target_info_.SetRefrain(true, v1);
            DEBUG_OUT("#1 CurvePlan:refranin two section valid; "
                      << ",vmax=" << vmax
                      << ",v0=" << v0 << ",v1=" << v1
                      << ",amax=" << amax << ",jmax=" << jmax
                      << ",q0=" << q0 << ",q1Old=" << q1Old << ",v1Old=" << v1Old << ",minQ=" << minQ
                      << ",dec_q=" << dec_q << ",q1q0=" << q1q0
                      << ",later:q1=" << q1 << ",v1=" << v1

            );
        } else {
            global_plan_.target_info_.SetRefrain(false, v1);
            DEBUG_OUT("#2 CurvePlan:refranin two section is not exist; "
                      << ",vmax=" << vmax
                      << ",v0=" << v0 << ",v1=" << v1
                      << ",amax=" << amax << ",jmax=" << jmax
                      << ",q0=" << q0 << ",q1=" << q1 << ",minQ=" << minQ
                      << ",dec_q=" << dec_q << ",q1q0=" << q1q0
                      << ",v1=" << v1

            );
        }
    } else {
        // 小于等于最大限制速度，则进行两次规划了
        global_plan_.target_info_.SetRefrain(false, v1);
        DEBUG_OUT("#3 CurvePlan:refranin two section is not exist: v0< vmax; "
                  << ",vmax=" << vmax
                  << ",v0=" << v0 << ",v1=" << v1
                  << ",amax=" << amax << ",jmax=" << jmax
                  << ",q0=" << q0 << ",q1=" << q1 << ",minQ=" << minQ

        );
    }
}

/**
 * @brief 检查是否需要最小距离减速重规划
 *
 * 该函数用于检查当前路径任务是否需要基于最小距离进行减速重规划。
 *
 * 主要逻辑包括：
 * 1. 检查规划目标信息是否有效。
 * 2. 判断当前规划是否已经是目标点停车。
 * 3. 检查当前是否正在运行静态动作。
 * 4. 检查工作缓冲区是否为空且当前无运行的行走任务。
 * 5. 根据当前操作模式，选择不同的重规划方式。
 * 6. 如果队列头数据存在且是导航码，则根据轨迹类型进行相应处理。
 *
 */
void TaskManager::CheckMinDisDecReplan()
{
    // 规划目标信息有效
    PlanTargetInfo::DataType info;
    if (!global_plan_.target_info_.GetInfo(info)) {
        DEBUG_OUT("CheckMinDisDecReplan: get info invalid");
        return;
    }

    // 当前规划已经是目标点停车，则不进行判断
    if (fabs(info.end_value) < 0.0001f) {
        DEBUG_OUT("CheckMinDisDecReplan: get info endValue is 0");
        return;
    }

    // 当上一次规划速度不是0，则进入判断
    //  如果当前正在运行--静态动作，不进行停车距离判断
    if (cur_op_) {
        DEBUG_OUT("can not need CheckMinDisDecReplan; curOp is running");
        return;
    }

    // 如果队列为空且当前无运行的行走任务(空闲)
    bool empty = work_buffer_.GetCount() == 0;
    if (empty && !cur_nv_) {
        DEBUG_WARN_OUT("CheckMinDisDecReplan: workbuffer is empty;cur nv is nullptr");
        return;
    }

    // 运动参数
    auto param = MotionParm::curve_params_;
    bool load_flag = MotionParm::load_flags_.current_load;
    auto load_vel = load_flag ? param.load : param.unload;
    DEBUG_OUT("CheckMinDisDecReplan: loadFlag=" << load_flag);

    // #1 如果当前时路径任务，如果是自旋，则跳过新的规划
    float cur_vel = fabs(record_last_nv_vel_.linear_), /*fCurW = fabs(record_last_nv_vel_.fAngular),*/ cur_q = 0.0f, range = 0.0f, max_vel = 0.0f;
    // #2 根据当前操作模式，选择不同的重规划方式
    auto &run_mode = global_plan_.plan_mode_;
    DEBUG_OUT("CheckMinDisDecReplan:enter new plan;mode=" << run_mode.Mode());
    if (cur_nv_) {
        // #1 自旋返回
        bool spin = cur_nv_->GetTrajType() == (short)TrajType::SPIN_TRAJ;
        if (spin) {
            DEBUG_OUT_THROTTLE(ROSNode()->get_logger(), 
                *(ROSNode()->get_clock()), 500, "CheckMinDisDecReplan:cur nv is spin");
            return;
        }

        cur_vel = fabs(cur_nv_->GetCurValue());
        // fCurW = fabs(cur_nv_->GetAngularVel());
        cur_q = cur_nv_->GetRelProgress();
        range = cur_nv_->GetTraj()->GetRange();
        max_vel = fabs(cur_nv_->GetMaxVel());
        // 如果是直线停车
        float cur_end_value = cur_nv_->GetEndValue();
        bool cur_stop = fabs(cur_end_value) < 0.0001f;
        // 更新规划目标信息
        short type = cur_nv_->GetTraj()->type_;
        auto start_id = cur_nv_->start_node_id_;
        auto end_id = cur_nv_->end_node_id_;
        if (empty || cur_stop) {
            DEBUG_OUT("CheckMinDisDecReplan: curnv is ready to stop:");
            return;
        } else {
            DEBUG_OUT("CheckMinDisDecReplan:cur nv continue re plan:"
                      << ",fCurVel=" << cur_vel << ",fCurQ=" << cur_q
                      << ",range=" << range << ",max_vel=" << max_vel
                      << ",type=" << type << ",start_id=" << start_id
                      << ",end_id=" << end_id);
        }
    }

    //////////////////////////////////
    // #2 如果是直线，曲线，侧移等，看是否需要和下一个任务串起来
    // 进入正常规划阶段
    DEBUG_OUT("CheckMinDisDecReplan: cur nv fCurVel=" << cur_vel);
    USHORT next_pos = work_buffer_.HeadPos(); // 查找队列头数据是否存在
    WorkCode *next_work_code = work_buffer_.PeekItem(next_pos);
    if (!next_work_code) {
        DEBUG_WARN_OUT("CheckMinDisDecReplan: next is null");
        return;
    }

    // // 初始值，是当前路段的距离
    float total = range; // 规划时总共路线的距离
    // // 计算减速距离
    double dec_q = global_plan_.DisRequirement(false, cur_vel, 0);
    double dec_q2 = global_plan_.GetStopDistance(cur_vel);
    DEBUG_OUT("minQDisDec: dec_q=" << dec_q << ",dec_q2=" << dec_q2 << ",fCurVel=" << cur_vel);

    while (next_work_code) {
        WorkCodeType type = next_work_code->type_;
        switch (type) {
            case WorkCodeType::NO_WORKCODE:
            case WorkCodeType::OP_WORKCODE:
            {
                DEBUG_ERROR_OUT("CheckMinDisDecReplan errro: first traj is nv_workcode");
                return;
            }
            break;
            case WorkCodeType::DYN_WORKCODE:
            {
                // 动态动作，忽略，进行下一路段查找
                DEBUG_WARN_OUT("CheckMinDisDecReplan: next head code is dyn op, jump this code,continue...");
            }
            break;
            case WorkCodeType::NV_WORKCODE:
            {
                // #2 正常逻辑，工作码有效且是导航码
                DEBUG_WARN_OUT("CheckMinDisDecReplan: find nv_code ok once");
                // 开始准备，累加路径长度
                TrajNv *next_nv = (TrajNv *)(next_work_code->pointer_);
                if (next_nv) {
                    Trajectory *traj = next_nv->GetTraj();
                    short type = traj->type_;
                    auto start_id = next_nv->start_node_id_;
                    auto end_id = next_nv->end_node_id_;
                    switch (type)
                    {
                        case (short)TrajType::UNDEFINED_TRAJ:
                        case (short)TrajType::SPIN_TRAJ:
                        {
                            DEBUG_ERROR_OUT("error: CheckMinDisDecReplan, (unknown traj:-1,spin:3),type=" << type);
                            return;
                        }
                        break;
                        case (short)TrajType::LINE_TRAJ:
                        case (short)TrajType::BEZIER_TRAJ:
                        case (short)TrajType::SIDE_TRAJ:
                        {
                            // 如果是直线停车
                            float end_vel = next_nv->GetEndValue();
                            bool stop = fabs(end_vel) < 0.0001f;
                            double curVmax = next_nv->GetMaxVel();
                            // 加上新的路径长度
                            total += traj->GetRange();
                            DEBUG_OUT("CheckMinDisDecReplan: total = " << total << ",dec_q=" << dec_q);
                            if (!stop)
                            {
                                DEBUG_OUT("CheckMinDisDecReplan: cur is not stop; continue find;");
                                continue;
                            }
                            // 当前路线需要规划停车了
                            if (total <= dec_q + 0.05)
                            {
                                DEBUG_OUT("CheckMinDisDecReplan agv ready stop: total = " << total << ",dec_q+0.05 = " << dec_q + 0.05);
                                // 系统参数
                                double vmax = std::min(curVmax, load_vel.line.vmax);
                                double amax = load_vel.line.amax, jmax = load_vel.line.jmax;
                                double q0 = cur_q, q1 = total, v0 = cur_vel, v1 = 0;

                                // 开始新的全速度局规划
                                StartNewGlobalPlan(vmax, amax, jmax, GetCycle(), q0, q1, v0, v1);
                                global_plan_.target_info_.SetPlanFlag(false);
                                global_plan_.target_info_.Update(type, start_id, end_id, v1);
                                DEBUG_OUT("CheckMinDisDecReplan traj stop: global plan:"
                                        << ",vmax=" << vmax
                                        << ",amax=" << amax
                                        << ",jmax=" << jmax
                                        << ",total=" << total
                                        << ",q0=" << q0 << ",q1=" << q1
                                        << ",v0=" << v0 << ",v1=" << v1
                                        << ",type=" << type << ",start_id=" << start_id << ",end_id=" << end_id);
                                return;
                            }
                        }
                        break;
                    }
                }
            }
            break;
        }

        next_pos = work_buffer_.NextEntry(next_pos);
        next_work_code = work_buffer_.PeekItem(next_pos);
    }

    DEBUG_WARN_OUT("CheckMinDisDecReplan: can not need");
}


////////////////////////////栈板识别相关函数////////////////////////////////////
/**
 * @brief 识别节点是否存在有效路径
 *
 * 判断给定货物节点ID是否存在于指定起点和终点之间，并检查路径是否有效。
 *
 * @param cargo_node_id 货物节点ID
 * @param start 起点节点信息
 * @param end 终点节点信息
 *
 * @return 如果货物节点ID存在于指定起点和终点之间，并且路径有效，则返回true；否则返回false。
 */
bool TaskManager::IdentifyNode(std::string cargo_node_id, agv_msgs::msg::MapNode start, agv_msgs::msg::MapNode end)
{
    bool ret = false;
    int start_id = 0, end_id = 0;

    if (cargo_node_id != end.nodeid) {
        DEBUG_OUT("IdentifyNode cargo_node_id != end.nodeid; cargo_node_id=" << cargo_node_id
                  << ", start_id=" << start.nodeid
                  << ", end_id=" << end.nodeid);
        return ret;
    }

    if (!string2Number(start.nodeid, start_id) || !string2Number(end.nodeid, end_id)) {
        DEBUG_ERROR_OUT("error: IdentifyNode to number failed; cargo_node_id=" << cargo_node_id
                         << ", start_id=" << start.nodeid
                         << ", end_id=" << end.nodeid);
        return ret;
    }

    Node* start_node = world.GetNode(start_id);
    Node* end_node = world.GetNode(end_id);
    bool find_path1 = world.IsPath(start_id, end_id);
    bool find_path2 = world.IsPath(end_id, start_id);
    bool find_node1 = start_node != nullptr;
    bool find_node2 = end_node != nullptr;

    if (!find_node1 || !find_node2 || !find_path1 || !find_path2) {
        DEBUG_ERROR_OUT("error: IdentifyNode map node or path does not exist; find_node1=" << find_node1
                         << ", find_node2=" << find_node2
                         << ", find_path1=" << find_path1
                         << ", find_path2=" << find_path2);
        return ret;
    }

    Node start_node_obj = *start_node;
    auto bits = start_node_obj.Bits();
    ret = TestBit(bits, INDENTIFY_NODE_BITS);
    DEBUG_OUT("need IdentifyNode? =" << ret << " start_id=" << start_id << " end_id=" << end_id);

    return ret;
}

/**
 * @brief 向任务管理器中插入一个识别动作
 *
 * 在任务管理器中插入一个识别动作，并返回操作是否成功。
 *
 * @param identify_node_id 识别节点的ID
 * @param cargo_node_id 货物节点的ID
 *
 * @return 如果操作成功，返回true；否则返回false
 */
bool TaskManager::InsertIdentifyAction(std::string identify_node_id, std::string cargo_node_id)
{
    agv_msgs::msg::Action action;
    action.blockingtype = OP_MUTEX_TYPE;
    action.actionid = "123456789";
    action.actiontype = "InsertIdentifyAction";

    if (dyn_virt_status_.loc_type_ != TagStateType::STATIC_LOCATION) {
        OSS_TO_STRING(task_err_detail_, "identifynodeid action op is failed; cur location is not static node");
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    int cur_id = 0;
    if (!string2Number(identify_node_id, cur_id)) {
        OSS_TO_STRING(task_err_detail_, "error: current identifynodeid is not number; virtulFromNode=" << dyn_virt_status_.from_node_);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    bool same_id = cur_id == dyn_virt_status_.from_node_;
    if (!same_id) {
        OSS_TO_STRING(task_err_detail_, "error: current identifynodeid is not virtualFromNode; virtulFromNode=" << dyn_virt_status_.from_node_
                         << ", cur_id=" << cur_id);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    // 添加起始点
    auto& params = action.actionparameters;
    params.clear();
    agv_msgs::msg::ActionParam param;
    param.key = KEY_START_ID;
    param.value = identify_node_id;
    params.push_back(param);
    param.key = KEY_END_ID;
    param.value = cargo_node_id;
    params.push_back(param);

    // 动作执行点
    Node* op_node = world.GetNode(cur_id);
    if (!op_node) {
        OSS_TO_STRING(task_err_detail_, "error: current identifynodeid action node is null; virtulFromNode=" << dyn_virt_status_.from_node_);
        DEBUG_ERROR_OUT(task_err_detail_);
        return false;
    }

    // normal op
    Operation* cur_op = nullptr;
    auto pallet_identify_op = new OperationPalletIdentify(action);

    // 绑定回调函数
    pallet_identify_op->GetRawPathCallBack = std::bind(&TaskManager::GetRawMapPath, this, PARAM_1, PARAM_2, PARAM_3);
    // pallet_identify_op->ModifyDoublePathCallBack = std::bind(&TaskManager::ModifyDoubleMapPath, this, PARAM_1, PARAM_2, PARAM_3);
    pallet_identify_op->ReplacePalletEndPathCallBack = std::bind(&TaskManager::ReplacePalletEndPath, this, PARAM_1, PARAM_2, PARAM_3);

    cur_op = pallet_identify_op;
    WorkCode work_code(WorkCodeType::OP_WORKCODE, reinterpret_cast<DWORD>(cur_op), task_status_);
    work_buffer_.Put(work_code, true);
    task_status_ = NORMAL_WORKCODE;
    dyn_virt_status_.last_is_op_ = true;  // if not return; it is op workcode need set m_bLastIsOp flag;

    // UpdateActionListStates(SetActionState(action, STATUS_WAIT));
    DEBUG_OUT("compile action op param: actiontype: " << action.actiontype << 
        ", actionid: " << action.actionid);
    return true;
}

/**
 * @brief 获取指定起点和终点之间的原始路径信息
 *
 * 获取指定起点和终点之间的原始路径信息，并将路径信息存储到提供的数组中。
 *
 * @param start 起点节点ID
 * @param end 终点节点ID
 * @param result 用于存储路径信息的数组
 *
 * @return 如果成功获取路径信息，则返回true；否则返回false
 */
bool TaskManager::GetRawMapPath(int start, int end, std::array<Posture, 2> &result)
{
    Node* start_node = world.GetNode(start);
    Node* end_node = world.GetNode(end);
    if (!start_node || !end_node) {
        DEBUG_ERROR_OUT("error: GetRawMapPath failed; node is null "
                << ",start=" << start << ",end=" << end);
        return false;
    }

    // 获取原始路径是否存在
    Path* path = world.GetPathPointer(start, end);
    if (!path) {
        DEBUG_ERROR_OUT("error: GetRawMapPath failed; path is null "
                << ",start=" << start << ",end=" << end);
        return false;
    }

    int type = path->type_;
    DEBUG_OUT("GetRawMapPath path.type(0:line 1:bezier:2:side)=" << type);

    // 获取原始路径点
    float x = 0, y = 0, theta = 0, quality = 0;
    bool success = GetLaserAutoPos(x, y, theta, quality, false, true) && quality > 5.0f;
    if (success) {
        DEBUG_OUT("GetRawMapPath use location pos x:" << x << " y:" << y << " theta:" << theta << " quality:" << quality);
        Angle heading(theta, AngleMode::InRadian);
        result[0] = Posture(x, y, heading);
    } else {
        DEBUG_OUT("GetRawMapPath use map pos x:" << start_node->x_ << " y:" << start_node->y_ << " theta:" << path->GetHeading(*start_node).Degree());
        result[0] = Posture(start_node->x_, start_node->y_, path->GetHeading(*start_node));
    }

    result[1] = Posture(end_node->x_, end_node->y_, path->GetHeading(*end_node));

    DEBUG_OUT("GetRawMapPath ok;" << ",start=" << start
        << ",end=" << end
        << ",result:"
        << "(" << result[0].x_
        << "," << result[0].y_
        << "," << result[0].theta_
        << ");"
        << "(" << result[1].x_
        << "," << result[1].y_
        << "," << result[1].theta_
        << ");");
    return true;
}

/**
 * @brief 修改双向路径
 *
 * 修改从起点到终点的双向路径，首先修改从起点到终点的路径，然后修改从终点到起点的路径。
 *
 * @param start 起点位置
 * @param end 终点位置
 * @param bezier_points 控制点数组，包含四个贝塞尔控制点
 * @return 修改成功返回true，失败返回false
 */
bool TaskManager::ModifyDoubleMapPath(int start, int end, std::array<Point, 4> bezier_points)
{
    // 第一条;进去路段替换,修正结束点
    if (!ModifyMapPath(start, end, bezier_points, true)) {
        DEBUG_ERROR_OUT("error: ModifyfirstMapPath failed;"
                        << ",start=" << start << ",end=" << end);
        return false;
    }

    // 第二条;回来路段替换，不修正起始点，只调整控制点系数
    auto bezier_points2 = bezier_points;
    // 反转路径处理
    bool reverse = ReverseCtrlPoints(bezier_points2);
    if (!reverse) {
        DEBUG_ERROR_OUT("error: ReverseCtrlPoints failed;"
                        << ",start=" << end << ",end=" << start);
        return false;
    }

    if (!ModifyMapPath(end, start, bezier_points2, false)) {
        DEBUG_ERROR_OUT("error: ModifysecondMapPath failed;"
                        << ",start=" << end << ",end=" << start);
        return false;
    }

    return true;
}

/**
 * @brief 反转贝塞尔曲线的控制点
 *
 * 将给定的贝塞尔曲线的控制点反转，并根据指定的系数进行调整。
 *
 * @param bezier_points 贝塞尔曲线的控制点数组，包含四个点（起点、两个控制点和终点）
 * @return 反转并调整成功返回true，否则返回false
 */
bool TaskManager::ReverseCtrlPoints(std::array<Point, 4> &bezier_points)
{
    // 反转系数
    auto param = MotionParm::smooth_params_;
    float k1 = param.cp1_k_;
    float k2 = param.cp2_k_;
    if (k1 <= 0.0f || k2 <= 0.0f || k1 >= 2.0f || k2 >= 2.0f) {
        DEBUG_ERROR_OUT("error: ReverseCtrlPoints failed; K is error number"
                        << ",k1=" << k1 << ",k2=" << k2);
        return false;
    }

    // 调整前
    std::array<Point, 4> temp_points;
    temp_points[0] = bezier_points[3]; // 终点变起点
    temp_points[1] = bezier_points[2]; // 控制点互换
    temp_points[2] = bezier_points[1];
    temp_points[3] = bezier_points[0]; // 起点变终点
    DEBUG_OUT("ReverseCtrlPoints raw; "
            << "temp_points[0]=" << temp_points[0].x_ << "," << temp_points[0].y_
            << ",temp_points[1]=" << temp_points[1].x_ << "," << temp_points[1].y_
            << ",temp_points[2]=" << temp_points[2].x_ << "," << temp_points[2].y_
            << ",temp_points[3]=" << temp_points[3].x_ << "," << temp_points[3].y_
            << ",k1=" << k1 << ",k2=" << k2);

    // 调整后
    Line start_line(temp_points[0], temp_points[1]);
    Line end_line(temp_points[3], temp_points[2]);
    temp_points[1] = start_line.TrajFun(start_line.Length() * k1);
    temp_points[2] = end_line.TrajFun(end_line.Length() * k2);
    DEBUG_OUT("ReverseCtrlPoints finish; "
            << "temp_points[0]=" << temp_points[0].x_ << "," << temp_points[0].y_
            << ",temp_points[1]=" << temp_points[1].x_ << "," << temp_points[1].y_
            << ",temp_points[2]=" << temp_points[2].x_ << "," << temp_points[2].y_
            << ",temp_points[3]=" << temp_points[3].x_ << "," << temp_points[3].y_
            << ",k1=" << k1 << ",k2=" << k2);

    bezier_points = temp_points;
    return true;
}

/**
 * @brief 修改地图路径
 *
 * 修改指定起止点之间的路径，包括生成新的贝塞尔路径和修改终点节点坐标（可选）。
 *
 * @param start 起始节点索引
 * @param end 终止节点索引
 * @param bezier_points 贝塞尔控制点数组，包含四个点
 * @param modify_end_node 是否修改终点节点坐标
 *
 * @return 修改成功返回true，失败返回false
 */
bool TaskManager::ModifyMapPath(int start, int end, std::array<Point, 4> bezier_points, bool modify_end_node)
{
    // 获取需要修改的起始点(起点不修正)
    Node* start_node = world.GetNode(start);
    Node* end_node = world.GetNode(end);
    if (!start_node || !end_node) {
        DEBUG_ERROR_OUT("error: ModifyMapPath failed; node is null "
                        << ",start=" << start << ",end=" << end);
        return false;
    }

    // 获取旧的路径是否存在
    Path* old_path = world.GetPathPointer(start, end);
    if (!old_path) {
        DEBUG_ERROR_OUT("error: ModifyMapPath failed; old_path is null "
                        << ",start=" << start << ",end=" << end);
        return false;
    }

    DEBUG_OUT("ModifyMapPath old_path.type(0:line 1:bezier:2:side)=" << static_cast<int>(old_path->type_));

    BezierPath* bezier_path = new BezierPath;
    bezier_path->type_ = static_cast<USHORT>(PathType::BEZIER_TYPE);
    bezier_path->id_ = old_path->id_;
    bezier_path->start_node_ = old_path->start_node_;
    bezier_path->end_node_ = old_path->end_node_;
    bezier_path->velocity_limit_ = old_path->velocity_limit_;
    bezier_path->guide_type_str_ = old_path->guide_type_str_;
    bezier_path->guide_type_ = old_path->guide_type_;
    bezier_path->max_dead_dist_ = MAX_DEAD_DIST;
    bezier_path->move_dir_ = old_path->move_dir_;
    bezier_path->stop_dist_empty_ = old_path->stop_dist_empty_;
    bezier_path->stop_dist_full_ = old_path->stop_dist_full_;
    bezier_path->detect_plan_ = old_path->detect_plan_;
    bezier_path->bits_ = old_path->bits_;
    bezier_path->word1_ = old_path->word1_;
    bezier_path->word2_ = old_path->word2_;

    // 起点不变
    auto start_point = bezier_points[0];
    (void)(start_point);
    bezier_path->ctrl_points_[0] = bezier_points[1];
    bezier_path->ctrl_points_[1] = bezier_points[2];
    auto end_point = bezier_points[3];
    DEBUG_OUT("ModifyMapPath ;" << "bezier_points[0]:x=" << bezier_points[0].x_ << ", y=" << bezier_points[0].y_
            << "bezier_points[1]:x=" << bezier_points[1].x_ << ", y=" << bezier_points[1].y_
            << "bezier_points[2]:x=" << bezier_points[2].x_ << ", y=" << bezier_points[2].y_
            << "bezier_points[3]:x=" << bezier_points[3].x_ << ", y=" << bezier_points[3].y_);

    // 修改目标节点坐标
    if (modify_end_node) {
        end_node->x_ = end_point.x_;
        end_node->y_ = end_point.y_;
        DEBUG_OUT("Replace end node ok ;x =" << end_point.x_ << ", y =" << end_point.y_);
    }

    DEBUG_OUT("Replace Path start");
    // 初始化新的路径后，新的覆盖旧路径，且删除旧的路径
    if (bezier_path != nullptr && bezier_path->Create()) {
        bool ok = world.OverWritePath(start, end, bezier_path);
        if (!ok) {
            DEBUG_ERROR_OUT("error: ModifyMapPath failed; OverwritePath failed "
                          << ",start=" << start << ",end=" << end);
            return false;
        }
    } else {
        DEBUG_OUT("new create bezier path is error(nullptr)");
        return false;
    }

    DEBUG_OUT("Replace Path finish");
    DEBUG_OUT("ModifyMapPath: BezierPath: " << ",Id=" << bezier_path->id_
            << ",trajType=" << bezier_path->type_
            << ",StartNode=" << bezier_path->start_node_
            << ",EndNode=" << bezier_path->end_node_
            << ",fVeloLimit=" << bezier_path->velocity_limit_
            << ",GuideType=" << bezier_path->guide_type_
            << ",fMaxDeadDist=" << bezier_path->max_dead_dist_
            << ",cp1.x=" << bezier_path->ctrl_points_[0].x_
            << ",cp1.y=" << bezier_path->ctrl_points_[0].y_
            << ",cp2.x=" << bezier_path->ctrl_points_[1].x_
            << ",cp2.y=" << bezier_path->ctrl_points_[1].y_
            << ",nDetectPlan " << bezier_path->detect_plan_
            << ",m_strGuideType =" << bezier_path->guide_type_str_
            << ",bShift=" << bezier_path->is_shift_
            << ",fAngHead=" << bezier_path->ang_head_
            << ",bits=" << bezier_path->bits_
            << ",word1=" << bezier_path->word1_
            << ",word2=" << bezier_path->word2_);
    return true;
}

/**
 * @brief 替换工作缓冲区中首个任务的路径终点
 *
 * 该函数尝试将工作缓冲区中首个任务的路径终点替换为新的起点和终点。
 *
 * @param start 新的起点节点ID
 * @param end   新的终点节点ID
 *
 * @return 成功替换返回true，否则返回false
 */
bool TaskManager::ReplaceTheEndPath(int start, int end)
{
    bool is_work_buffer_empty = work_buffer_.IsEmpty();
    // 下一个任务即是 #1到工位点->#2取货
    if (is_work_buffer_empty) {
        DEBUG_ERROR_OUT("error: ReplaceTheEndPath failed; WorkBuffer is Empty");
        return false;
    }

    WorkCode* peek_head_work_code = work_buffer_.PeekItem(work_buffer_.HeadPos());
    WorkCodeType type1 = peek_head_work_code ? peek_head_work_code->type_ : WorkCodeType::NO_WORKCODE;
    WorkCode* peek_head_work_code_copy = work_buffer_copy_.PeekItem(work_buffer_copy_.HeadPos());
    WorkCodeType type2 = peek_head_work_code_copy ? peek_head_work_code_copy->type_ :  WorkCodeType::NO_WORKCODE;
    bool is_nav_code = type1 ==  WorkCodeType::NV_WORKCODE && type2 ==  WorkCodeType::NV_WORKCODE;
    if (!is_nav_code) {
        DEBUG_ERROR_OUT("error: ReplaceTheEndPath failed; is_nav_code is false;"
                        << "type1=" << static_cast<int>(type1)
                        << ",type2=" << static_cast<int>(type2));
        return false;
    }

    TrajNv* old_traj_nv = reinterpret_cast<TrajNv*>(peek_head_work_code->pointer_);
    bool is_same_obj = peek_head_work_code->pointer_ == peek_head_work_code_copy->pointer_;
    if (!old_traj_nv || !is_same_obj) {
        DEBUG_ERROR_OUT("error: ReplaceTheEndPath failed; old_traj_nv is null or workbuffer != workbuffercopy;");
        return false;
    }

    bool is_same = old_traj_nv->start_node_id_ == start && old_traj_nv->end_node_id_ == end;
    if (!is_same) {
        DEBUG_ERROR_OUT("error: ReplaceTheEndPath failed;" << "buffer:start=" << old_traj_nv->start_node_id_
                                                           << ",buffer:end=" << old_traj_nv->end_node_id_
                                                           << ",new start=" << start
                                                           << ",end=" << end);
        return false;
    }

    TrajNv* new_traj_nv = AddNodeToNodeTraj(start, end);
    if (!new_traj_nv) {
        DEBUG_ERROR_OUT("error: ReplaceTheEndPath failed; new_traj_nv is null");
        return false;
    }

    // 删除旧的对象
    DEBUG_OUT("DelOldNvTask delete start");
    delete old_traj_nv;
    old_traj_nv = nullptr;
    DEBUG_OUT("DelOldNvTask, delete finish");

    peek_head_work_code->pointer_ = reinterpret_cast<DWORD>(new_traj_nv);
    peek_head_work_code_copy->pointer_ = reinterpret_cast<DWORD>(new_traj_nv);
    DEBUG_OUT("ReplaceTheEndPath finish; start=" << start << ", end=" << end);
    return true;
}

/**
 * @brief 替换托盘末端路径
 *
 * 替换从起始节点到终止节点的托盘末端路径为贝塞尔曲线路径。
 *
 * @param start 起始节点ID
 * @param end 终止节点ID
 * @param bezier_points 贝塞尔曲线的四个控制点
 *
 * @return 替换成功返回true，失败返回false
 */
bool TaskManager::ReplacePalletEndPath(int start, int end, std::array<Point, 4> bezier_points)
{
    bool is_work_buffer_empty = work_buffer_.IsEmpty();
    // 下一个任务即是 #1到工位点->#2取货
    if (is_work_buffer_empty) {
        DEBUG_ERROR_OUT("error: ReplacePalletEndPath failed; WorkBuffer is Empty");
        return false;
    }

    WorkCode* peek_head_work_code = FindHeadNvWorkCode();
    WorkCode* peek_head_work_code_copy = FindHeadNvWorkCodeCopy();
    if (peek_head_work_code == nullptr || peek_head_work_code_copy == nullptr) {
        DEBUG_ERROR_OUT("error: ReplacePalletEndPath failed; cannot find next nv workcode.");
        return false;
    }

    TrajNv* old_traj_nv = reinterpret_cast<TrajNv*>(peek_head_work_code->pointer_);
    bool is_same_obj = peek_head_work_code->pointer_ == peek_head_work_code_copy->pointer_;
    if (!old_traj_nv || !is_same_obj) {
        DEBUG_ERROR_OUT("error: ReplacePalletEndPath failed; old_traj_nv is null or workbuffer != workbuffercopy;");
        return false;
    }

    bool is_same = old_traj_nv->start_node_id_ == start && old_traj_nv->end_node_id_ == end;
    if (!is_same) {
        DEBUG_ERROR_OUT("error: ReplacePalletEndPath failed;" << "buffer:start=" << old_traj_nv->start_node_id_
                                                              << ",buffer:end=" << old_traj_nv->end_node_id_
                                                              << ",new start=" << start
                                                              << ",end=" << end);
        return false;
    }

    Node* start_node = world.GetNode(start);
    Node* end_node = world.GetNode(end);
    if (!start_node || !end_node) {
        DEBUG_ERROR_OUT("error: ReplacePalletEndPath failed; node is null "
                        << ",start=" << start << ",end=" << end);
        return false;
    }

    // 获取旧的路径是否存在
    Path* old_path = world.GetPathPointer(start, end);
    if (!old_path) {
        DEBUG_ERROR_OUT("error: ReplacePalletEndPath failed; old_path is null "
                        << ",start=" << start << ",end=" << end);
        return false;
    }

    BezierPath* bezier_path = new BezierPath;
    bezier_path->type_ = static_cast<USHORT>(PathType::BEZIER_TYPE);
    bezier_path->id_ = old_path->id_;
    bezier_path->start_node_ = old_path->start_node_;
    bezier_path->end_node_ = old_path->end_node_;
    bezier_path->velocity_limit_ = old_path->velocity_limit_;
    bezier_path->guide_type_str_ = old_path->guide_type_str_;
    bezier_path->guide_type_ = old_path->guide_type_;
    bezier_path->max_dead_dist_ = MAX_DEAD_DIST;
    bezier_path->move_dir_ = old_path->move_dir_;
    bezier_path->stop_dist_empty_ = old_path->stop_dist_empty_;
    bezier_path->stop_dist_full_ = old_path->stop_dist_full_;
    bezier_path->detect_plan_ = old_path->detect_plan_;
    bezier_path->bits_ = old_path->bits_;
    bezier_path->word1_ = old_path->word1_;
    bezier_path->word2_ = old_path->word2_;

    // 起点不变
    auto start_point = bezier_points[0];
    (void)(start_point);
    bezier_path->ctrl_points_[0] = bezier_points[1];
    bezier_path->ctrl_points_[1] = bezier_points[2];
    auto end_point = bezier_points[3];
    RCLCPP_INFO(rclcpp::get_logger("AgvNavigate"), "[ReplacePalletEndPath] start[%.4f %.4f] point1[%.4f %.4f] point2[%.4f %.4f] end[%.4f %.4f]", 
        start_point.x_, start_point.y_, bezier_path->ctrl_points_[0].x_, 
        bezier_path->ctrl_points_[0].y_, bezier_path->ctrl_points_[1].x_,
        bezier_path->ctrl_points_[1].y_, end_point.x_, end_point.y_);

    Node bezier_end = *end_node;
    bezier_end.x_ = end_point.x_;
    bezier_end.y_ = end_point.y_;
    RCLCPP_INFO(rclcpp::get_logger("AgvNavigate"), "[ReplacePalletEndPath] bezierEnd[%.4f %.4f]", bezier_end.x_, bezier_end.y_);

    TrajNv* new_traj_nv = AddNodeToNodeTrajWithPath(*start_node, bezier_end, bezier_path);
    if (!new_traj_nv) {
        DEBUG_ERROR_OUT("error: ReplacePalletEndPath failed; new_traj_nv is null");
        return false;
    }

    // 删除旧的对象
    DEBUG_OUT("DelOldNvTask delete start");
    delete old_traj_nv;
    old_traj_nv = nullptr;
    DEBUG_OUT("DelOldNvTask, delete finish");

    peek_head_work_code->pointer_ = reinterpret_cast<DWORD>(new_traj_nv);
    peek_head_work_code_copy->pointer_ = reinterpret_cast<DWORD>(new_traj_nv);
    DEBUG_OUT("ReplaceTheEndPath finish; start=" << start << ", end=" << end);
    return true;
}

/**
 * @brief 将节点添加到路径轨迹中
 *
 * 将一个节点从起始节点到目标节点添加到给定的路径轨迹中，并返回轨迹指针。
 *
 * @param from_node 起始节点
 * @param to_node 目标节点
 * @param path 路径对象指针
 *
 * @return 成功返回轨迹指针，失败返回nullptr
 */
TrajNv *TaskManager::AddNodeToNodeTrajWithPath(Node &from_node, Node &to_node, Path *path)
{
    // 任务已经编译到了库位点
    USHORT cargo_node = dyn_virt_status_.from_node_;
    bool is_static_point = dyn_virt_status_.loc_type_ == TagStateType::STATIC_LOCATION;
    (void)(cargo_node);
    (void)(is_static_point);

    if (path == nullptr) {
        DEBUG_ERROR_OUT("error: compile AddNodeToNodeTraj: path is null;");
        return nullptr;
    }

    RCLCPP_INFO(rclcpp::get_logger("AgvNavigate"), "[AddNodeToNodeTrajWithPath] to_node[%.4f %.4f]", to_node.x_, to_node.y_);
    bool use_temp_end_point = true;
    TrajNv* traj_nv = new TrajNv(path, to_node, 0.0f, path->Size(), level_, false, use_speed_level_, use_temp_end_point);
    if (traj_nv == nullptr) {
        DEBUG_ERROR_OUT("error: AddNvObj traj_nv is null");
        return nullptr;
    }

    if (!traj_nv->GetTraj()) {
        DEBUG_ERROR_OUT("error: AddNvObj traj_nv->GetTraj is null");
        return nullptr;
    }

    return traj_nv;
}

/**
 * @brief 向节点轨迹中添加节点
 *
 * 将指定节点添加到节点轨迹中，并返回新的TrajNv指针。
 *
 * @param from_node 起始节点编号
 * @param to_node 目标节点编号
 *
 * @return 成功时返回新的TrajNv指针，失败时返回nullptr
 */
TrajNv *TaskManager::AddNodeToNodeTraj(USHORT from_node, USHORT to_node)
{
    // 任务已经编译到了库位点
    USHORT cargo_node = dyn_virt_status_.from_node_;
    bool is_static_point = dyn_virt_status_.loc_type_ == TagStateType::STATIC_LOCATION;

    if (to_node != cargo_node || !is_static_point) {
        DEBUG_ERROR_OUT("error: AddNodeToNodeTraj failed; from_node" << from_node << ", to_node:" << to_node
            << ", cargo_node=" << cargo_node << ", static_point=" << is_static_point);
        return nullptr;
    }

    Path* path = world.GetPathPointer(from_node, to_node);
    if (path == nullptr) {
        DEBUG_ERROR_OUT("error: compile AddNodeToNodeTraj: path is null;"
            << "from_node=" << from_node << ", to_node=" << to_node);
        return nullptr;
    }

    Node* node_to = world.GetNode(to_node);
    if (!node_to) {
        DEBUG_ERROR_OUT("error: compile AddNodeToNodeTraj: node_to is null "
                        << ", to_node=" << to_node);
        return nullptr;
    }

    Node to_node_copy = *node_to;
    TrajNv* nv = AddNvObj(path, to_node_copy, 0.0f, path->Size());
    if (nv == nullptr) {
        DEBUG_ERROR_OUT("error: AddNodeToNodeTraj nv is null"
                        << ", to_node=" << to_node);
        return nullptr;
    }

    UpdateVirtualStaticPoseByPst(nv, to_node);
    return nv;
}


/**
 * @brief 向任务管理器中添加一个新的导航对象
 *
 * 在任务管理器中添加一个新的导航对象。该对象将按照指定的路径从起点移动到终点。
 *
 * @param path 路径对象指针，表示导航对象移动的路径
 * @param node_to 目标节点引用，表示导航对象要到达的终点
 * @param from 起点位置，表示导航对象在路径上的起始位置
 * @param to 终点位置，表示导航对象在路径上的结束位置
 * @param force_check_mark 是否强制检查标记，如果为true，则忽略路径上已有的标记
 *
 * @return 返回新创建的导航对象指针，如果创建失败则返回nullptr
 */
TrajNv *TaskManager::AddNvObj(Path *path, Node &node_to, float from, float to, bool force_check_mark)
{

    DEBUG_OUT("ready AddNvObj new nv obj , node to: " << node_to.id_);
    TrajNv* traj_nv = new TrajNv(path, node_to, from, to, level_, force_check_mark, use_speed_level_);
    if (traj_nv == nullptr) {
        DEBUG_ERROR_OUT("error: AddNvObj traj_nv is null");
        return nullptr;
    }

    if (!traj_nv->GetTraj()) {
        DEBUG_ERROR_OUT("error: AddNvObj traj_nv->GetTraj is null");
        return nullptr;
    }

    return traj_nv;
}